Revision history [back]

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.