Контекст: у меня есть большая арена 2 м x 2 м, на которой напечатаны 4 маркера aruco, их положение из угла арены известно и фиксировано. Теперь у меня есть еще один маркер aruco на роботе, движущемся по этой арене. PS известная позиция находится в 2d.
Проблема: я хочу найти положение робота на арене (относительно известного угла арены). Я использую python для того же, сначала обнаруживая маркеры с изображения с помощью DetectMarker (), а затем оценивают положение маркеров. Значения tve c, возвращаемые функцией оценки позы, дают положение маркера относительно системы координат камеры, что отлично работает, если камера перпендикулярна арене, но когда камера удерживается под углом, возникает большая ошибка в позиции.
Правильно ли мой подход? Считайте, что камера откалибрована хорошо, что является источником ошибки?
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, actual_size, mtx, dist)
cv2.imshow('img',img)
index = np.where(ids==0) # getting the known aruco marker
rotation_matrix[:3, :3], _ = cv2.Rodrigues(rvec[index]) # Computing Rotational Matrix
for i in range(3):
rotation_matrix[i][3]= tvec[index][0][i] # Adding Translation Values to it
inverse_rot = np.linalg.inv(rotation_matrix) # Inversing the matrix
for i,j,k in zip(ids,tvec,rvec):
print(i,'POS:',j) # prints id and tvec values
pt[:3] = j.reshape(3,1)
rot_point = np.dot(inverse_rot,pt) # Homogeneous matrix . tvec values
print(rot_point[:3]) # The new position
print(np.sqrt(rot_point[0]**2 + rot_point[1]**2 )) # Distance
rotational_matrix - это матрица 4x4, содержащая вращение, перемещение, которая используется для переноса системы координат ) в один из маркеров (известный маркер на арене , поэтому этот маркер становится исходной точкой) и преобразование других точек (tvecs в системе камеры) в систему маркеров.
Однородное преобразование координат
