estimatePoseBoard from TD and save RT matrix

estimatePoseBoard_cv2_td_saveRT.py

# -load camera instrinsic matrix
cameraMatrix = np.loadtxt("calib/cameraMatrix.csv",delimiter=",")
distCoeffs = np.loadtxt("calib/distCoeffs.csv",delimiter=",")


# prepare the 'same' aruco board
aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
board = cv2.aruco.GridBoard_create(5, 5, 0.2,0.1,dictionary)
# retval	=	cv.aruco.GridBoard_create(	markersX, markersY, markerLength, markerSeparation, dictionary[
# markerLength, markerSeparationは、meter単位で、カメラで撮影するサイズを同じにする

#reading image from TD
arr = op('imageL').numpyArray('delayed=True')
gray = arr[:, :, 0]
gray = gray * 255.0
gray = gray.astype(np.uint8)
gray = cv2.flip(gray, 0) #TD - CV coordinate flip


# detecting  aruco.detectMarkers
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
print('len(ids)', len(ids))


if(len(ids) > 0):
	retval, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, None, None)

	#ロドリゲス公式で、rvecから回転行列へ変換する
	Rmatrix, Jacob = cv2.Rodrigues(rvec)

	# [3 x 4 matrix]で、R|t マトリクスを作成
	RTmatrix34_L = np.concatenate([Rmatrix, tvec],  axis=1)

	PM_L = RTmatrix34_L
	np.save('PM_L.npy',PM_L)