Position de la caméra dans les coordonnées du monde à partir de cv :: resolPnP

J’ai une caméra calibrée (masortingce insortingnsèque et coefficients de distorsion) et je souhaite connaître la position de la caméra en connaissant des points 3d et leurs points correspondants dans l’image (2d points).

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

Je dois donc trouver la rotation / traduction de la caméra dans le système de coordonnées mondial.

À partir des liens ci-dessus, il semble que le code soit simple, en python:

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

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

  • rvec, la sortie tvec de solvePnP est une masortingce 3×1, 3 vecteurs d’éléments
  • cv2.Rodrigues (rvec) est une masortingce 3×3
  • cv2.Rodrigues (rvec) [0] est une masortingce 3×1, 3 vecteurs d’élément
  • cameraPosition est une multiplication masortingcielle 3×1 * 1×3 qui est une masortingce .. 3×3. Comment puis-je l’utiliser dans opengl avec de simples glTranslatef et glRotate ?

Si avec “coordonnées du monde” vous voulez dire “coordonnées de l’object”, vous devez obtenir la transformation inverse du résultat fourni par l’algorithme pnp.

Il existe un truc pour inverser les masortingces de transformation qui vous permet de sauvegarder l’opération d’inversion, ce qui est généralement coûteux, et cela explique le code en Python. Étant donné une transformation [R|t] , nous avons cette inv([R|t]) = [R'|-R'*t] , où R' est la transposée de R Donc, vous pouvez 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 = Rt(); // 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 masortingx 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 généralement 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 en OpenGL est: X pointe vers la droite, Y vers le haut, Z vers l’arrière (comme dans cette image ). Donc, vous devez appliquer une rotation autour de l’axe X de 180 degrés. La formule de cette masortingce de rotation est en wikipedia .

 // T is your 4x4 masortingx in the OpenCV frame cv::Mat RotX = ...; // 4x4 masortingx 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 ceci avec un grain de sel.

Enfin, tenez compte du fait que les masortingces dans OpenCV sont stockées dans l’ordre des lignes principales dans la mémoire, et celles dans OpenGL, dans l’ordre des colonnes majeures.

Si vous souhaitez en faire une masortingce de pose 4×4 standard en spécifiant la position de votre caméra. Utilisez rotM en haut à gauche 3×3, tvec en tant que 3 éléments à droite et 0,0,0,1 en bas

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

puis inverser (pour obtenir la pose de la caméra au lieu de la pose du monde)