Относительное вращение между позой (rvec) - PullRequest
Новая
       18

Относительное вращение между позой (rvec)

0 голосов
/ 31 августа 2018

Я хочу найти относительные углы поворота между двумя маркерами Aruco, используя python и cv2. Я называю свои маркеры маркером "test" и маркером "reference".

Я успешно получил позу маркеров, используя cv2.aruco.estimatePoseSingleMarkers. Это дает мне «test_rvec» для маркера теста и «ref_rvec» для маркера ссылки.

Насколько я понимаю, rvec (тот же формат, что и в cv2.solvePnP, который, как я полагаю, использует aruco под крышками) - это поворот маркера относительно камеры. Таким образом, чтобы получить вращение теста маркеров относительно опорных маркеров, я: 

R_ref_to_cam = cv2.Rodrigues(ref_rvec)[0] #reference to camera
R_test_to_cam = cv2.Rodrigues(test_rvec)[0] #test to camera
R_cam_to_ref = np.transpose(R_ref_to_cam) #inverse of reference to camera
R_test_to_ref = np.matmul(R_test_to_cam,R_cam_to_ref) #test to reference

Затем я использую cv2.decomposeProjectionMatrix для вычисления углов Эйлера полученной матрицы (R_test_to_ref).

При тестировании, когда оба маркера расположены на столе ровно и с одинаковой ориентацией, а камера направлена ​​прямо вниз, я получаю X = 0, Y = 0, Z = 0, как и ожидалось, поскольку относительная ориентация между маркерами равна ноль.

Однако, если я поверну один маркер на 90 градусов в направлении "z" (все еще держа его на своем столе), я получу X = 30, Y = 30, Z = 90. Я ожидал бы, что две оси сообщают о 90 градусах, а третья (ось вращения) сообщает о 0 градусах. Что я делаю не так?

Ответы [ 2 ]

0 голосов
/ 06 сентября 2018

Я сомневаюсь, что вы должны использовать cv2.decomposeProjectionMatrix () для R_test_to_ref, потому что это матрица вращения 3x3, а не матрица проекции. Одна ссылка для преобразования матрицы вращения 3x3 в углы Эйлера в Python: здесь , код скопирован ниже: 

# Code from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :

    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

Что вы используете для углов Эйлера? Вам не нужны углы Эйлера для OpenGL, например, вы можете использовать матрицы напрямую. У углов Эйлера есть свои проблемы, и во многих случаях вам повезет больше с вектором вращения или кватернионом в качестве компактного представления вращения.

0 голосов
/ 05 сентября 2018

Я не уверен, правильно ли я понимаю. Но в отличие от вас, когда я получал справку, я использовал базовую векторную математику: AB = AC - BC enter image description here A и B - маркеры, C - камера. поэтому, если я инвертирую ссылку tvec & rvec, добавлю к первым маркерам, я получу относительную точку. К этому я написал две функции: 

def inversePerspective(rvec, tvec):
""" Applies perspective transform for given rvec and tvec. """
    R, _ = cv2.Rodrigues(rvec)
    R = np.matrix(R).T
    invTvec = np.dot(R, np.matrix(-tvec))
    invRvec, _ = cv2.Rodrigues(R)
    return invRvec, invTvec



def relativePosition(rvec1, tvec1, rvec2, tvec2):
""" Get relative position for rvec2 & tvec2. Compose the returned rvec & tvec to use composeRT with rvec2 & tvec2 """
    rvec1, tvec1 = rvec1.reshape((3, 1)), tvec1.reshape((3, 1))
    rvec2, tvec2 = rvec2.reshape((3, 1)), tvec2.reshape((3, 1))

    # Inverse the second marker, the right one in the image
    invRvec, invTvec = inversePerspective(rvec2, tvec2)

    info = cv2.composeRT(rvec1, tvec1, invRvec, invTvec)
    composedRvec, composedTvec = info[0], info[1]

    composedRvec = composedRvec.reshape((3, 1))
    composedTvec = composedTvec.reshape((3, 1))
    return composedRvec, composedTvec

Я в основном использовал для обозначения точки. Я использовал один из маркеров вращения в этом случае, но, возможно, он может помочь вам. Я действительно написал это в своем блоге, но это на турецком языке. Извините, я не успел написать по-английски: Мой блог

А также вы можете увидеть мой исходный код для этого, я надеюсь, что он может помочь вам: Мой исходный код.

Извините, если я неправильно понимаю вашу проблему там. Я не проверял свой код для точного определения угла, я проверял его на перевод.

...