我正在尝试构建一个立体视觉charuco姿势检测。我已经能够进行立体校准,我有两台摄像机的固有矩阵。为了便于开发,我对两台摄像机使用相同的校准参数(它们完全相同,水平距离约20厘米) 下面的代码块是到目前为止我已经能够组合起来的,应该可以估计出检测到的标记在世界空间中的姿势(x,y,z)欧几里德坐标
(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)
我在每个摄影机的视图中独立地显示标记,这是可行的,但当标记位于两个同时不失真的点的视野中时,会向我抛出这个错误
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'
以下是CalibleCameracharuco为每个摄像头提供的mtx1和mtx2内部参数
[[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]]
我之所以只展示一个,是因为它们对于每个摄像头都是相同的(摄像头是相同的,出于开发目的,我对这两个摄像头都使用相同的图像)。我尝试了np.array()来隐藏,但仍然存在相同的问题
有什么想法吗
目前没有回答
相关问题 更多 >
编程相关推荐