OpenCV + OpenGL: proper camera pose using solvePnP

I've got problem with obtaining proper camera pose from iPad camera using OpenCV.

I'm using custom made 2D marker (based on AruCo library ) - I want to render 3D cube over that marker using OpenGL.

In order to recieve camera pose I'm using solvePnP function from OpenCV.

According to THIS LINK I'm doing it like this:

cv::solvePnP(markerObjectPoints, imagePoints, [self currentCameraMatrix], _userDefaultsManager.distCoeffs, rvec, tvec);

tvec.at<double>(0, 0) *= -1; // I don't know why I have to do it, but translation in X axis is inverted

cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3

R = R.t();  // rotation of inverse
tvec = -R * tvec; // translation of inverse

cv::Mat T(4, 4, R.type()); // T is 4x4
T(cv::Range(0, 3), cv::Range(0, 3)) = R * 1; // copies R into T
T(cv::Range(0, 3), cv::Range(3, 4)) = tvec * 1; // copies tvec into T
double *p = T.ptr<double>(3);
p[0] = p[1] = p[2] = 0;
p[3] = 1;

camera matrix & dist coefficients are coming from findChessboardCorners function, imagePoints are manually detected corners of marker (you can see them as green square in the video posted below), and markerObjectPoints are manually hardcoded points that represents marker corners:

markerObjectPoints.push_back(cv::Point3d(-6, -6, 0));
markerObjectPoints.push_back(cv::Point3d(6, -6, 0));
markerObjectPoints.push_back(cv::Point3d(6, 6, 0));
markerObjectPoints.push_back(cv::Point3d(-6, 6, 0));

Because marker is 12 cm long in real world, I've chosed the same size in the for easier debugging.

As a result I'm recieving 4x4 matrix T, that I'll use as ModelView matrix in OpenCV. Using GLKit drawing function looks more or less like this:

- (void)glkView:(GLKView *)view drawInRect:(CGRect)rect {
    // preparations
    glClearColor(0.0, 0.0, 0.0, 0.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    float aspect = fabsf(self.bounds.size.width / self.bounds.size.height);
    effect.transform.projectionMatrix = GLKMatrix4MakePerspective(GLKMathDegreesToRadians(39), aspect, 0.1f, 1000.0f);

    // set modelViewMatrix
    float mat[16] = generateOpenGLMatFromFromOpenCVMat(T);
    currentModelMatrix = GLKMatrix4MakeWithArrayAndTranspose(mat);
    effect.transform.modelviewMatrix = currentModelMatrix;

    [effect prepareToDraw];

    glDrawArrays(GL_TRIANGLES, 0, 36); // draw previously prepared cube
}

I'm not rotating everything for 180 degrees around X axis (as it was mentioned in previously linked article), because I doesn't look as necessary.

The problem is that it doesn't work! Translation vector looks OK, but X and Y rotations are messed up :(

I've recorded a video presenting that issue:

http://www.youtube.com/watch?v=EMNBT5H7-os

I've tried almost everything (including inverting all axises one by one), but nothing actually works.

What should I do? How should I properly display that 3D cube? Translation / rotation vectors that come from solvePnP are looking reasonable, so I guess that I can't correctly map these vectors to OpenGL matrices.

Answers


Thanks to Djo1509 from http://answers.opencv.org/ I've found out that the problem was unnecessary transposed rotation matrix R matrix used as part of matrix T, and unnecessary tvec = -R * tvec operation.

For more info look there


Need Your Help

How to reduce this IF-Else ladder in c#

c# .net asp.net linq asp.net-controls

This is the IF -Else ladder which I have created to focus first visible control on my form.According to the requirement any control can be hidden on the form.So i had to find first visible control ...

Is there a way to launch an aggressive and complete garbage collection in Java?

java garbage-collection

For memory optimization reasons, I'm launching myself the garbage collector during profiling, to check if objects are correctly cleaned after disposing of them.