带有 OpenCV 和 OpenGL 的 AR

Question

问题出在这里：我编写了一个代码来在一张纸上显示 OpenGL 茶壶并进行绘图。为此，我跟踪了论文的 4 个角（使用 SURF 检测和匹配，然后计算单应矩阵，然后移动角位置的平均值以减少抖动）。角坐标用于计算相机的内在和外在矩阵（分别使用calibrateCamera() 和solvePnP()）。然后使用Rodrigues() 计算旋转矩阵。之后，我使用decomposeProjectionMatrix() 计算了旋转角度。这是代码的 OpenCV 部分：

...
objPoints.push_back(objCorners);
scenePoints.push_back(sceneCorners);
calibrateCamera(objPoints, scenePoints, Size(640,480), camMtx, distortCoeff, RVecs, tVecs);
solvePnP(objCorners, sceneCorners, camMtx, distortCoeff, RVec, tVec);
Rodrigues(RVec, rotMtx);
getAngles(rotMtx, rotAngles);

objCorners 是模板图像中的角坐标（[1 1]，[img width 1]，[img width img height]，[1 img height]）。 sceneCorners 是网络摄像头帧中的角坐标，使用单应矩阵计算。函数getAngles()如下：

void getAngles(Mat &rotCamMtx, Vec3d &angles)
{
    Mat camMtx, rotMtx, transVec, rotMtxX, rotMtxY, rotMtxZ;
    double  *r = rotCamMtx.ptr<double>();
    double projMtx[12] = {r[0], r[1], r[2], 0, 
                          r[3], r[4], r[5], 0, 
                          r[6], r[7], r[8], 0};

    decomposeProjectionMatrix(Mat(3,4,CV_64FC1,projMtx), camMtx, rotMtx, transVec, rotMtxX, rotMtxY, rotMtxZ, angles);
}

然后我设置OpenGL模型视图矩阵的元素如下：

modelViewMat[0]  = 1.0;
modelViewMat[1]  = 0.0;
modelViewMat[2]  = 0.0;
modelViewMat[3]  = 0.0;
modelViewMat[4]  = 0.0;
modelViewMat[5]  = 1.0;
modelViewMat[6]  = 0.0;
modelViewMat[7]  = 0.0;
modelViewMat[8]  = 0.0;
modelViewMat[9]  = 0.0;
modelViewMat[10] = 1.0;
modelViewMat[11] = 0.0;
modelViewMat[12] = 2*matCenter.x/639 - 641/639;
modelViewMat[13] = 481/479 - 2*matCenter.y/479;
modelViewMat[14] = -0.25;
modelViewMat[15] = 1.0;

matCenter是纸的中心坐标，取4个角的平均值。 modelViewMat[12] 和 modelViewMat[13] 中的值是通过将像素坐标 ([1 640], [1 480]) 映射到 ([-1 1], [1 -1]) 获得的。 OpenGL部分代码：

...
glMatrixMode(GL_PROJECTION);
glLoadIdentity();

glMatrixMode(GL_MODELVIEW);
glLoadMatrixd(modelViewMat);

glRotated(-45, 1.0, 0.0, 0.0);
glRotated(rotAngles[2], 0.0, 1.0, 0.0);

glShadeModel(GL_SMOOTH);
glColor3f(1.0, 1.0, 1.0);
glutSolidTeapot(0.3);

我将茶壶绕 x 轴旋转 -45 度，使其看起来“坐在”纸上。 结果是这样的：如果我翻译桌子上的纸，茶壶在纸上的位置或多或少是正确的（在同一个地方）。如果我旋转纸张，茶壶将正确地跟随旋转（围绕 y 轴），但位置不再正确。问题是：如何将茶壶“钉”在纸的同一位置？我试过直接在OpenGL模型视图矩阵中使用Rodrigues()和solvePnP()的结果（如OpenCV + OpenGL: proper camera pose using solvePnP所建议的那样），但结果不正确。

Answer 1

几天前根据http://blog.yarrago.com/2011/08/introduction-to-augmented-reality.html的代码解决了这个问题。要正确显示 3D 对象，首先设置 OpenGL 投影矩阵，然后设置 OpenGL 模型视图矩阵。投影矩阵的元素由相机的固有矩阵计算如下：

calibrateCamera(objPoints, scenePoints, Size(640,480), camMtx, distortCoeff, RVecs, tVecs);
...
projectionMat[0]  = 2*camMtx.at<double>(0,0)/frameW;
projectionMat[1]  = 0;
projectionMat[2]  = 0;
projectionMat[3]  = 0;
projectionMat[4]  = 0;
projectionMat[5]  = 2*camMtx.at<double>(1,1)/frameH;
projectionMat[6]  = 0;
projectionMat[7]  = 0;
projectionMat[8]  = 1 - 2*camMtx.at<double>(0,2)/frameW;
projectionMat[9]  = -1 + (2*camMtx.at<double>(1,2) + 2)/frameH;
projectionMat[10] = (zNear + zFar)/(zNear - zFar);
projectionMat[11] = -1;
projectionMat[12] = 0;
projectionMat[13] = 0;
projectionMat[14] = 2*zNear*zFar/(zNear - zFar);
projectionMat[15] = 0;

frameW 和 frameH 分别是 640 和 480。 zNear 是 0.1 和 zFar 是 100。

OpenGL 模型视图矩阵的元素由旋转矩阵和平移向量计算得出（从solvePnP() 和Rodrigues() 获得）。为了获得 3D 对象的正确定位，需要在计算模型视图矩阵之前转换平移向量。

// Offset value to move the translation vector
double offsetC[3][1] = {424, 600, 0};
Mat    offset(3, 1, CV_64F, offsetC);
...
solvePnP(objCorners, sceneCorners, camMtx, distortCoeff, RVec, tVec);
Rodrigues(RVec, rotMtx);
tVec = tVec + rotMtx*offset;    // Move tVec to refer to the center of the paper
tVec = tVec / 250.0;            // Converting pixel coordinates to OpenGL world coordinates
...
modelviewMat[0]  = rotMtx.at<double>(0,0);
modelviewMat[1]  = -rotMtx.at<double>(1,0);
modelviewMat[2]  = -rotMtx.at<double>(2,0);
modelviewMat[3]  = 0;
modelviewMat[4]  = rotMtx.at<double>(0,1);
modelviewMat[5]  = -rotMtx.at<double>(1,1);
modelviewMat[6]  = -rotMtx.at<double>(2,1);
modelviewMat[7]  = 0;
modelviewMat[8]  = rotMtx.at<double>(0,2);
modelviewMat[9]  = -rotMtx.at<double>(1,2);
modelviewMat[10] = -rotMtx.at<double>(2,2);
modelviewMat[11] = 0;
modelviewMat[12] = tVec.at<double>(0,0);
modelviewMat[13] = -tVec.at<double>(1,0);
modelviewMat[14] = -tVec.at<double>(2,0);
modelviewMat[15] = 1;

offsetC 的数值是纸张中心的像素坐标。那么OpenGL部分的代码就是：

glMatrixMode(GL_PROJECTION);
glLoadMatrixf(projectionMat);

glMatrixMode(GL_MODELVIEW);
glLoadMatrixf(modelviewMat);

glRotatef(90, -1.0, 0.0, 0.0);  // Rotate the teapot first so that it will be displayed correctly on the paper
glutSolidTeapot(1.0);

茶壶正确定位的一个重要方面是tVec的变换。