Я пытаюсь построить обнаружение позы чаруко стереозрения. Я смог стереокалибровать, и у меня есть свои собственные матрицы с обеих камер. для простоты разработки я использую одни и те же параметры калибровки для обеих камер (они абсолютно одинаковы и установлены вместе на расстоянии около 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)
Я показываю маркер в поле зрения каждой камеры независимо, и это работает, но когда маркер находится в поле зрения для обоих одновременно, undistortPoints выдает мне эту ошибку.
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'
вот встроенные функции mtx1 и mtx2 от calibrateCameraCharuco для каждой камеры
[[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() скрыть, но все еще та же проблема.
Какие-нибудь мысли?
undistortPoints()
- это матрица преобразования выпрямления (матрица вращения 3x3), однако вы используете внутреннюю матрицу. docs.opencv.org/4.4.0/d9/d0c/ - person sebasth   schedule 03.08.2021undistortPoints()
, если заданы правильные значения R и P, функция вернет точки в виде координат выпрямленного изображения (u', v'). Если это то, что вы хотите, вы можете рассчитать R и P для обеих камер, используяstereoRectify()
в начале. - person sebasth   schedule 05.08.2021