Оценка позы камеры, дающая неправильные результаты

Question

Я пытаюсь оценить относительное движение камеры на основе совпадения точек на двух разных изображениях.Как и здесь: Оценка позы камеры: как интерпретировать поворот и матрицы перевода?

Но расчетный перевод и поворот не имеют смысла.

Я использую синтетическиевведите, чтобы убедиться, что все точки верны и идеально расположены.

10 x 10 x 10 точек равномерно распределены внутри куба.(Куб изображен с голубой передней панелью, красной задней панелью, более светлой верхней и более темной нижней частью)

zeroProjection Камера находится перед кубом, указывая на переднюю грань.

rotate90projection Камера слева от куба, указывающая на левую боковую грань.

Я строю две проекции.Вы можете легко визуально проверить, что камера сместилась на 90 градусов и переместилась по диагонали в плоскости xz между двумя проекциями.

В коде поворот (в градусах) задается как (0, -90, 0)

Перевод равен (0,7071, 0, 0,7071), расстояние перемещения камеры равно 1.

Затем я нахожу findEssentialMat () и recoverPose () для наборов 2d точек, чтобы получить перевод иоценки вращения.

Я ожидаю увидеть тот же перевод и вращение, которые я использовал для генерации изображений, но оценки совершенно неверны:

rotation estimate: (-74.86565284711004, -48.52201867665918, 121.26023708879158)
translation estimate: [[0.96576997]
 [0.17203598]
 [0.19414426]]

Как восстановить фактическое значение (0, -90, 0), (0,7071, 0, 0,7071) преобразование?

Полный код, который отображает два изображения куба и выводит оценки:

import cv2
import numpy as np
import math


def cameraMatrix(f, w, h):
    return np.array([
                     [f, 0, w/2],
                     [0, f, h/2],
                     [0, 0, 1]])


n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)


def cube(x=0, y=0, z=0, radius=1):
    c = np.zeros((n * n * n, 3), dtype=np.float32)
    for i in range(0, n):
        for j in range(0, n):
            for k in range(0, n):
                index = i + j * n + k * n * n
                c[index] = [i, j, k]
    c = 2 * c / (n - 1) - 1
    c *= radius
    c += [x, y, z]
    return c


def project3dTo2dArray(points3d, K, rotation, translation):
    imagePoints, _ = cv2.projectPoints(points3d,
                                       rotation,
                                       translation,
                                       K,
                                       np.array([]))
    p2d = imagePoints.reshape((imagePoints.shape[0],2))
    return p2d


def estimate_pose(projectionA, projectionB):
    E, _ = cv2.findEssentialMat(projectionA, projectionB, focal = f)
    _, r, t, _ = cv2.recoverPose(E,  projectionA, projectionB)
    angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
    print('rotation estimate:', angles)
    print('translation estimate:', t)


def main():
    c = cube(0, 0, math.sqrt(.5), 0.1)
    rotation = np.array([[0], [0], [0]], dtype=np.float32)
    translation = np.array([[0], [0], [0]], dtype=np.float32)
    zeroProjection = project3dTo2dArray(c, K, rotation, translation)
    displayCube(w, h, zeroProjection)

    rotation = np.array([[0], [-90], [0]], dtype=np.float32)
    translation = np.array([[math.sqrt(.5)], [0], [math.sqrt(.5)]], dtype=np.float32)
    print('applying rotation: ', rotation)
    print('applying translation: ', translation)
    rotate90projection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
    displayCube(w, h, rotate90projection)

    estimate_pose(zeroProjection, rotate90projection)


def displayCube(w, h, points):
    img = np.zeros((h, w, 3), dtype=np.uint8)

    plotCube(img, points)

    cv2.imshow('img', img)
    k = cv2.waitKey(0) & 0xff
    if k == ord('q'):
        exit(0)


def plotCube(img, points):
    # Red back face
    cv2.line(img, tuple(points[n*n*(n-1)]),         tuple(points[n*n*(n-1)+n-1]),         (0, 0, 255), 2)
    cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
    cv2.line(img, tuple(points[n*n*(n-1)]),         tuple(points[n*n*(n-1)+n*(n-1)]),     (0, 0, 200), 2)
    cv2.line(img, tuple(points[n*n*(n-1)+n-1]),     tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)

    # gray connectors
    cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
    cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
    cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
    cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)

    # Blue front face
    cv2.line(img, tuple(points[0]),       tuple(points[n-1]),         (255, 0, 0), 2)
    cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
    cv2.line(img, tuple(points[0]),       tuple(points[n*(n-1)]),     (200, 0, 0), 2)
    cv2.line(img, tuple(points[n-1]),     tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)


main()

Answer 1

Оказалось, что в моем коде было несколько незначительных ошибок (например, неправильная основная точка). Рабочий код ниже показывает 3 изображения.

Первый - это куб, отображаемый перед камерой. Второй - это тот же куб, но другая проекция. Камера была перемещена на 1 единицу и вращалась вокруг всех 3 осей. Перемещение и поворот камеры оцениваются по двум проекциям. Третий показывает куб, спроектированный с использованием оценок поворота и перемещения.

Код работает, так как второе и третье изображения похожи.

import cv2
import numpy as np
import math


def cameraMatrix(f, w, h):
    return np.array([
                     [f, 0, w/2],
                     [0, f, h/2],
                     [0, 0, 1]])


n = 10
f = 300
w = 640
h = 480
K = cameraMatrix(f, w, h)


def cube(x=0, y=0, z=0, radius=1):
    c = np.zeros((n * n * n, 3), dtype=np.float32)
    for i in range(0, n):
        for j in range(0, n):
            for k in range(0, n):
                index = i + j * n + k * n * n
                c[index] = [i, j, k]
    c = 2 * c / (n - 1) - 1
    c *= radius
    c += [x, y, z]
    return c


def project3dTo2dArray(points3d, K, rotation, translation):
    imagePoints, _ = cv2.projectPoints(points3d,
                                       rotation,
                                       translation,
                                       K,
                                       np.array([]))
    p2d = imagePoints.reshape((imagePoints.shape[0],2))
    return p2d


def estimate_pose(projectionA, projectionB):
    principal_point = (w/2,h/2)
    E, m = cv2.findEssentialMat(projectionA, projectionB, focal = f, pp = principal_point, method=cv2.RANSAC, threshold=1, prob=0.999)
    _, r, t, _ = cv2.recoverPose(E,  projectionA, projectionB, focal = f, pp = principal_point, mask = m)
    angles, _, _, _, _, _ = cv2.RQDecomp3x3(r)
    return angles, t


def main():
    c = cube(0, 0, math.sqrt(.5), 0.1)
    rotation = np.array([[0], [0], [0]], dtype=np.float32)
    translation = np.array([[0], [0], [0]], dtype=np.float32)
    zeroProjection = project3dTo2dArray(c, K, rotation, translation)
    displayCube(w, h, zeroProjection)

    rotation = np.array([[10], [-30], [5]], dtype=np.float32)
    translation = np.array([[math.sqrt(.7)], [0], [math.sqrt(.3)]], dtype=np.float32)

    print('applying rotation: ', rotation)
    print('applying translation: ', translation)
    movedprojection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
    displayCube(w, h, movedprojection)

    estRot, estTra= estimate_pose(zeroProjection, movedprojection)
    print('rotation estimate:', estRot)
    print('translation estimate:', estTra)

    rotation = np.array([[estRot[0]], [estRot[1]], [estRot[2]]], dtype=np.float32)
    translation = np.array([[estTra[0]], [estTra[1]], [estTra[2]]], dtype=np.float32)
    estimateProjection = project3dTo2dArray(c, K, rotation * math.pi / 180, translation)
    displayCube(w, h, estimateProjection)


def displayCube(w, h, points):
    img = np.zeros((h, w, 3), dtype=np.uint8)

    plotCube(img, points)

    cv2.imshow('img', img)
    k = cv2.waitKey(0) & 0xff
    if k == ord('q'):
        exit(0)


def plotCube(img, points):
    # Red back face
    cv2.line(img, tuple(points[n*n*(n-1)]),         tuple(points[n*n*(n-1)+n-1]),         (0, 0, 255), 2)
    cv2.line(img, tuple(points[n*n*(n-1)+n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 128), 2)
    cv2.line(img, tuple(points[n*n*(n-1)]),         tuple(points[n*n*(n-1)+n*(n-1)]),     (0, 0, 200), 2)
    cv2.line(img, tuple(points[n*n*(n-1)+n-1]),     tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (0, 0, 200), 2)

    # gray connectors
    cv2.line(img, tuple(points[0]), tuple(points[n*n*(n-1)]), (150, 150, 150), 2)
    cv2.line(img, tuple(points[n-1]), tuple(points[n*n*(n-1)+n-1]), (150, 150, 150), 2)
    cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*n*(n-1)+n*(n-1)]), (100, 100, 100), 2)
    cv2.line(img, tuple(points[n*(n-1)+n-1]), tuple(points[n*n*(n-1)+n*(n-1)+n-1]), (100, 100, 100), 2)

    # Blue front face
    cv2.line(img, tuple(points[0]),       tuple(points[n-1]),         (255, 0, 0), 2)
    cv2.line(img, tuple(points[n*(n-1)]), tuple(points[n*(n-1)+n-1]), (128, 0, 0), 2)
    cv2.line(img, tuple(points[0]),       tuple(points[n*(n-1)]),     (200, 0, 0), 2)
    cv2.line(img, tuple(points[n-1]),     tuple(points[n*(n-1)+n-1]), (200, 0, 0), 2)


main()