Skip to content

Instantly share code, notes, and snippets.

@shks

shks/opencv_rvec_tvec_to_matrix.py

Created September 2, 2021 02:42
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save shks/e3cd5fc22ad9f4ddbccd5312a64803c3 to your computer and use it in GitHub Desktop.
Save shks/e3cd5fc22ad9f4ddbccd5312a64803c3 to your computer and use it in GitHub Desktop.
Download ZIP
opencv rvec, tvec into matrix34 and 44 / opencvの姿勢推定で得たrvec, tvecベクトルをマトリクス表現へ変換する
Raw
opencv_rvec_tvec_to_matrix.py
retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, None, None)
#rvec, tvec : 3 float vector
#ロドリゲス公式で、rvecから回転行列へ変換する
Rmatrix, Jacob = cv2.Rodrigues(rvec)
# [3 x 4 matrix]で、R|t マトリクスを作成
RTmatrix34_L = np.concatenate([Rmatrix, tvec], axis=1)
# [4x4]のマトリクスにしておく
tmp = np.array([0,0,0,1])
RTmatrix = np.vstack((RTmatrix34_L, tmp.T))
#test with inverse / 逆行列のテスト
inv_RTmatrix = np.linalg.inv(RTmatrix)
#単位行列になるはず
print(cameraMatrix.dot(RTmatrix34_L))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment