web-dev-qa-db-fra.com

Position de la caméra en coordonnées mondiales à partir de cv :: resolverPnP

J'ai une caméra calibrée (matrice intrinsèque et coefficients de distorsion) et je veux connaître la position de la caméra en connaissant certains points 3D et leurs points correspondants dans l'image (points 2D).

Je le sais cv::solvePnP pourrait m'aider, et après avoir lu this et this je comprends que les sorties de resolPnP rvec et tvec sont la rotation et translation de l'objet dans le système de coordonnées de la caméra.

J'ai donc besoin de trouver la rotation/translation de la caméra dans le système de coordonnées mondial.

D'après les liens ci-dessus, il semble que le code soit simple, en python:

found,rvec,tvec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_coefs)
rotM = cv2.Rodrigues(rvec)[0]
cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)

Je ne connais pas les trucs python/numpy (j'utilise C++) mais cela n'a pas beaucoup de sens pour moi:

  • rvec, sortie tvec de resolPnP sont des matrices 3x1, des vecteurs à 3 éléments
  • cv2.Rodrigues (rvec) est une matrice 3x3
  • cv2.Rodrigues (rvec) [0] est une matrice 3x1, vecteurs à 3 éléments
  • cameraPosition est une multiplication matricielle 3x1 * 1x3 qui est une matrice .. 3x3. comment puis-je l'utiliser dans opengl avec de simples appels glTranslatef et glRotate?
c++openglopencvcomputer-visionpose-estimation
48
nkint

Si avec "coordonnées du monde" vous voulez dire "coordonnées d'objet", vous devez obtenir la transformation inverse du résultat donné par l'algorithme pnp.

Il existe une astuce pour inverser les matrices de transformation qui vous permet d'enregistrer l'opération d'inversion, qui est généralement coûteuse, et qui explique le code en Python. Étant donné une transformation [R|t], Nous avons cette inv([R|t]) = [R'|-R'*t], où R' Est la transposition de R. Vous pouvez donc coder (non testé):

cv::Mat rvec, tvec;
solvePnP(..., rvec, tvec, ...);
// rvec is 3x1, tvec is 3x1

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 = cv::Mat::eye(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

// T is a 4x4 matrix with the pose of the camera in the object frame

Mise à jour: Plus tard, pour utiliser T avec OpenGL, vous devez garder à l'esprit que les axes du cadre de la caméra diffèrent entre OpenCV et OpenGL .

OpenCV utilise la référence habituellement utilisée en vision par ordinateur: X pointe vers la droite, Y vers le bas, Z vers l'avant (comme dans cette image ). Le cadre de la caméra dans OpenGL est: X pointe vers la droite, Y vers le haut, Z vers l'arrière (comme dans le côté gauche de cette image ). Vous devez donc appliquer une rotation autour de l'axe X de 180 degrés. La formule de cette matrice de rotation est en wikipedia .

// T is your 4x4 matrix in the OpenCV frame
cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X
cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame

Ces transformations sont toujours déroutantes et je peux me tromper à un moment donné, alors prenez cela avec un grain de sel.

Enfin, tenez compte du fait que les matrices dans OpenCV sont stockées dans l'ordre des lignes principales en mémoire, et celles d'OpenGL, dans l'ordre des colonnes principales.

49
ChronoTrigger

Si vous souhaitez le transformer en une matrice de pose 4x4 standard spécifiant la position de votre caméra. Utilisez rotM comme carré 3x3 en haut à gauche, tvec comme 3 éléments à droite et 0,0,0,1 comme rangée du bas

pose = [rotation   tvec(0)
        matrix     tvec(1)
        here       tvec(2)
        0  , 0, 0,  1]

puis inversez-le (pour obtenir la pose de la caméra au lieu de la pose du monde)

4
Hammer