openCV undistortPoints matrix operand is an empty matrix

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?

Recommended topics

Hot tags