Calcul de la pose de la caméra avec une matrice d'homographie basée sur 4 points coplanaires

Question

J'ai 4 points coplanaires dans une vidéo (ou une image) représentant un quadrilatère (pas nécessairement un carré ou un rectangle) et je voudrais pouvoir afficher un cube virtuel au-dessus d'eux où les coins du cube se trouvent exactement sur les coins du quadrilatère vidéo.

Puisque les points sont coplanaires, je peux calculer l'homographie entre les coins d'un carré unitaire (c'est-à-dire [0,0] [0,1] [1,0] [1,1]) et les coordonnées vidéo du carré.

À partir de cette homographie, je devrais pouvoir calculer une pose correcte de la caméra, c'est-à-dire [R|t] où R est une matrice de rotation 3x3 et t un vecteur de translation 3x1, de sorte que le cube virtuel se trouve sur le quadrant vidéo.

J'ai lu de nombreuses solutions (dont certaines sur SO) et j'ai essayé de les mettre en œuvre, mais elles ne semblent fonctionner que dans certains cas "simples" (comme lorsque le quad vidéo est un carré) mais ne fonctionnent pas dans la plupart des cas.

Voici les méthodes que j'ai essayées (la plupart d'entre elles sont basées sur les mêmes principes, seuls les calculs de la translation sont légèrement différents). Soit K la matrice intrinsèque de la caméra et H l'homographie. On calcule :

A = K-1 * H

Soit a1,a2,a3 les vecteurs colonnes de A et r1,r2,r3 les vecteurs colonnes de la matrice de rotation R.

r1 = a1 / ||a1||
r2 = a2 / ||a2||
r3 = r1 x r2
t = a3 / sqrt(||a1||*||a2||)

Le problème est que cela ne fonctionne pas dans la plupart des cas. Afin de vérifier mes résultats, j'ai comparé R et t avec ceux obtenus par la méthode solvePnP d'OpenCV (en utilisant les points 3D suivants [0,0,0] [0,1,0] [1,0,0] [1,1,0]).

Comme j'affiche le cube de la même manière, j'ai remarqué que dans tous les cas solvePnP fournit des résultats corrects, alors que la pose obtenue à partir de l'homographie est la plupart du temps fausse.

En théorie, puisque mes points sont coplanaires, il est possible de calculer la pose à partir d'une homographie, mais je n'ai pas trouvé la manière correcte de calculer la pose à partir de H.

Avez-vous une idée de ce que je fais mal ?

Modifier après avoir essayé la méthode de @Jav_Rock

Salut Jav_Rock, merci beaucoup pour votre réponse, j'ai essayé votre approche (et beaucoup d'autres aussi) qui semble être plus ou moins OK. Néanmoins, j'ai encore quelques problèmes lorsque je calcule la pose sur la base de 4 points coplanaires. Afin de vérifier les résultats, je les compare avec ceux de solvePnP (qui seront bien meilleurs grâce à l'approche itérative de minimisation des erreurs de reprojection).

Voici un exemple :

• Cube jaune : Résoudre PNP
• Black Cube : La technique de Jav_Rock
• Cube(s) cyan (et violet) : quelques autres techniques donnant exactement les mêmes résultats

Comme vous pouvez le voir, le cube noir est plus ou moins OK mais ne semble pas bien proportionné, bien que les vecteurs semblent orthonormés.

EDIT2 : J'ai normalisé v3 après l'avoir calculé (afin d'appliquer l'orthonormalité) et cela semble résoudre certains problèmes également.

Answer 1

Si vous avez votre Homographie, vous pouvez calculer la pose de la caméra avec quelque chose comme ceci :

void cameraPoseFromHomography(const Mat& H, Mat& pose)
{
    pose = Mat::eye(3, 4, CV_32FC1);      // 3x4 matrix, the camera pose
    float norm1 = (float)norm(H.col(0));  
    float norm2 = (float)norm(H.col(1));  
    float tnorm = (norm1 + norm2) / 2.0f; // Normalization value

    Mat p1 = H.col(0);       // Pointer to first column of H
    Mat p2 = pose.col(0);    // Pointer to first column of pose (empty)

    cv::normalize(p1, p2);   // Normalize the rotation, and copies the column to pose

    p1 = H.col(1);           // Pointer to second column of H
    p2 = pose.col(1);        // Pointer to second column of pose (empty)

    cv::normalize(p1, p2);   // Normalize the rotation and copies the column to pose

    p1 = pose.col(0);
    p2 = pose.col(1);

    Mat p3 = p1.cross(p2);   // Computes the cross-product of p1 and p2
    Mat c2 = pose.col(2);    // Pointer to third column of pose
    p3.copyTo(c2);       // Third column is the crossproduct of columns one and two

    pose.col(3) = H.col(2) / tnorm;  //vector t [R|t] is the last column of pose
}

Cette méthode fonctionne pour moi. Bonne chance.

Answer 2

La réponse proposée par Jav_Rock ne fournit pas une solution valable pour les poses de caméra dans un espace tridimensionnel.

Pour estimer une transformation et une rotation arborescentes induites par une homographie, il existe de multiples approches. L'un d'eux fournit des formules fermées pour décomposer l'homographie, mais elles sont très complexes. De plus, les solutions ne sont jamais uniques.

Heureusement, OpenCV 3 implémente déjà cette décomposition ( DécomposerHomographieMat ). Étant donné une homographie et une matrice intrinsèque correctement mise à l'échelle, la fonction fournit un ensemble de quatre rotations et translations possibles.

Answer 3

Juste au cas où quelqu'un aurait besoin de porter en python la fonction écrite par @Jav_Rock :

def cameraPoseFromHomography(H):
    H1 = H[:, 0]
    H2 = H[:, 1]
    H3 = np.cross(H1, H2)

    norm1 = np.linalg.norm(H1)
    norm2 = np.linalg.norm(H2)
    tnorm = (norm1 + norm2) / 2.0;

    T = H[:, 2] / tnorm
    return np.mat([H1, H2, H3, T])

Fonctionne bien dans mes tâches.

Answer 4

Le calcul de [R|T] à partir de la matrice d'homographie est un peu plus compliqué que la réponse de Jav_Rock.

Dans OpenCV 3.0, il existe une méthode appelée cv::decomposeHomographyMat qui renvoie quatre solutions potentielles, dont une est correcte. Cependant, OpenCV n'a pas fourni de méthode pour choisir la solution correcte.

Je suis en train de travailler sur ce sujet et je posterai peut-être mes codes ici dans le courant du mois.

Answer 5

Le plan qui contient votre carré sur l'image a une ligne de fuite pour les agents de votre caméra. L'équation de cette ligne est A x+B y+C=0.

La normale de votre avion est (A,B,C) !

Soit p00,p01,p10,p11 les coordonnées du point après application des paramètres intrinsèques de la caméra et sous forme homogène, par exemple, p00=(x00,y00,1).

La ligne de fuite peut être calculée comme suit :

• vers le bas = p00 croise p01 ;
• gauche = p00 traverse p10 ;
• droite = p01 traverse p11 ;
• up = p10 croise p11 ;
• v1=gauche croix droite ;
• v2=up cross down ;
• vanish_line = v1 croise v2 ;

Où croix en produit vectoriel croisé standard