(repost from my question here: python - openCV undistortPoints matrix operand is an empty matrix - Stack Overflow)
I am trying to construct a stereo vision charuco pose detection. I have been able to stereoCalibrate and I have my intrinsic matrices from both cameras. for sake of development ease I am using the same calibration parameters for both cameras (they are exactly the same and mounted together about 20 cm apart horizontally)
the code block below is what I have been able to put together so far that should estimate the pose (x,y,z) euclidian coordinates in the world space of the detected marker.
(retStereo, L_intrinsics, L_distortion, R_intrinsics, R_distortion, R, T, essentialMatrix, fundamentalMatrix) = context.scene.calibration_data
codec = 0x47504A4D # MJPG
cap_right = cv2.VideoCapture(0)
cap_right.set(cv2.CAP_PROP_FOURCC, codec)
cap_left = cv2.VideoCapture(1)
cap_left.set(cv2.CAP_PROP_FOURCC, codec)
while(cap_right.isOpened() and cap_left.isOpened()):
succes_right, frame_right = cap_right.read()
succes_left, frame_left = cap_left.read()
cornersR, idsR, rejected_img_pointsR = aruco.detectMarkers(frame_right, ARUCO_DICT, parameters=ARUCO_PARAMETERS, cameraMatrix=context.scene.cameraMatrix, distCoeff=context.scene.distCoeffs)
cv2.aruco.drawDetectedMarkers(frame_right,cornersR,idsR)
if np.all(idsR is not None):
for i in range(0, len(idsR)):
R_rvec, R_tvec, R_markerPoints = aruco.estimatePoseSingleMarkers(cornersR[i], context.scene.cal_board_markerLength, context.scene.cameraMatrix, context.scene.distCoeffs)
(R_rvec - R_tvec).any() # get rid of that nasty numpy value array error
aruco.drawAxis(frame_right, context.scene.cameraMatrix, context.scene.distCoeffs, R_rvec, R_tvec, 0.05) # Draw Axis
#convert charuco corners to chessboard corners
retR, cornersR, corner_ids_R = cv2.aruco.interpolateCornersCharuco(cornersR, idsR, frame_right, CHARUCO_BOARD)
cornersL, idsL, rejected_img_pointsL = aruco.detectMarkers(frame_left, ARUCO_DICT, parameters=ARUCO_PARAMETERS, cameraMatrix=context.scene.cameraMatrix, distCoeff=context.scene.distCoeffs)
cv2.aruco.drawDetectedMarkers(frame_left,cornersL,idsL)
if np.all(idsL is not None):
for i in range(0, len(idsL)):
L_rvec, L_tvec, L_markerPoints = aruco.estimatePoseSingleMarkers(cornersL[i], context.scene.cal_board_markerLength, context.scene.cameraMatrix, context.scene.distCoeffs)
(L_rvec - L_tvec).any() # get rid of that nasty numpy value array error
aruco.drawAxis(frame_left, context.scene.cameraMatrix, context.scene.distCoeffs, L_rvec, L_tvec, 0.05) # Draw Axis
#convert charuco corners to chessboard corners
retL, cornersL, corner_ids_L = cv2.aruco.interpolateCornersCharuco(cornersL, idsL, frame_left, CHARUCO_BOARD)
if np.all(idsL is not None) and np.all(idsR is not None):
mtx1 = np.array(L_intrinsics)
mtx2 = np.array(R_intrinsics)
print(mtx1, mtx2)
dist1 = L_distortion
dist2 = R_distortion
projMat1 = mtx1 @ cv2.hconcat([np.eye(3), np.zeros((3,1))]) # Cam1 is the origin
projMat2 = mtx2 @ cv2.hconcat([R, T]) # R, T from stereoCalibrate
# points1 is a (N, 1, 2) float32 from cornerSubPix
points1u = cv2.undistortPoints(cornersR, mtx1, dist1, None, mtx1)
points2u = cv2.undistortPoints(cornersL, mtx2, dist2, None, mtx2)
points4d = cv2.triangulatePoints(projMat1, projMat2, points1u, points2u)
points3d = (points4d[:3, :]/points4d[3, :]).T
print(points3d)
I show the marker in each camera’s view independently and that works but when the marker is in the field of view for both simultaneously undistortPoints throws this error at me
points1u = cv2.undistortPoints(cornersR, mtx1, dist1, None, mtx1)
cv2.error: OpenCV(4.4.0) C:\Users\appveyor\AppData\Local\Temp\1\pip-req-build-q0nmoxxv\opencv\modules\core\src\matrix_expressions.cpp:24:
error: (-5:Bad argument) Matrix operand is an empty matrix. in function 'cv::checkOperandsExist'
here are the mtx1 and mtx2 intrinsics from calibrateCameraCharuco for each camera
[[3.34162537e+03 0.00000000e+00 2.03190826e+03]
[0.00000000e+00 3.34045368e+03 1.12097255e+03]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
I’m only showing one because they are the same for each camera (cameras are identical and for dev purposes, I’m using the same images for both). I tried np.array() to covert but still the same issue.
any thoughts?