AR avec OpenCV & OpenGL

Question

Voici le problème: J'ai écrit un code pour afficher la théière OpenGL sur une feuille de papier avec dessin. Pour cela, je parcours les 4 coins du papier (en utilisant la détection SURF & correspondant suivi par le calcul de la matrice d'homographie, puis la moyenne mobile de la position des coins pour réduire la gigue). Les coordonnées des coins sont utilisées pour calculer les matrices extrinsèques intrinsèques de la caméra (en utilisant calibrateCamera() et solvePnP(), respectivement). La matrice de rotation est ensuite calculée en utilisant Rodrigues(). Ensuite, j'ai calculé les angles de rotation en utilisant decomposeProjectionMatrix(). Voici la partie OpenCV du code:

... 
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 sont les coins coordonnées dans l'image modèle ([1 1], [img largeur 1], [img largeur img hauteur], [1 hauteur img]). sceneCorners sont les coordonnées des coins dans le cadre de la webcam, calculées à l'aide de la matrice d'homographie. La fonction getAngles() est la suivante:

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); 
} 

Ensuite, je mets l'élément de la matrice de vue du modèle OpenGL comme suit:

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 est le centre de coordonnées du papier, obtenue en prenant la moyenne les 4 coins. Les valeurs dans modelViewMat[12] et modelViewMat[13] sont obtenues en mappant les coordonnées de pixels ([1 640], [1 480]) à ([-1 1], [1 -1]). La partie OpenGL du code:

... 
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); 

Je tourné la théière -45 degrés autour de l'axe x pour le rendre apparaît « assis » sur le papier. Le résultat est le suivant: si je traduis le papier sur le bureau, l'emplacement de la théière sur le papier est plus ou moins correct (au même endroit). Si je fais tourner le papier, la théière suivra la rotation correctement (autour de l'axe y), mais l'emplacement n'est plus correct. La question est: comment "épingler" la théière toujours au même endroit du papier? J'ai essayé d'utiliser le résultat de Rodrigues() et solvePnP() directement dans la matrice de vue de modèle OpenGL (comme suggéré dans OpenCV + OpenGL: proper camera pose using solvePnP), mais le résultat est incorrect.

Answer 1

Résolu ce problème il y a plusieurs jours, basé sur le code de http://blog.yarrago.com/2011/08/introduction-to-augmented-reality.html. Pour afficher correctement l'objet 3D, la matrice de projection OpenGL est définie en premier, suivie de la matrice de vue du modèle OpenGL. Les éléments de la matrice de projection sont calculées à partir de la matrice intrinsèque de l'appareil comme suit:

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 et frameH sont 640 et 480, respectivement. zNear est 0.1 et zFar est 100. Les éléments de la matrice de vue de modèle OpenGL sont calculés à partir de la matrice de rotation et du vecteur de traduction (obtenus à partir de solvePnP() et Rodrigues()). Pour obtenir un positionnement correct de l'objet 3D, le vecteur de traduction doit être transformé avant de calculer la matrice de vue du modèle.

// 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; 

Les valeurs numériques pour offsetC est le pixel de coordonnées du centre de la feuille.La partie OpenGL du code est alors:

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); 

Une chose importante pour le positionnement correct de la théière est la transformation de tVec.