Как рассчитать координаты трехмерного мира для каждого пикселя изображения, используя параметры калибровки камеры? - PullRequest
Новая
       25

Как рассчитать координаты трехмерного мира для каждого пикселя изображения, используя параметры калибровки камеры?

0 голосов
/ 26 апреля 2019

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

py=0.0036 # pixel width and height px py
px=0.0036
focal_length=1777.341982433
fx=focal_length
fy=focal_length

dist_offset=112.6882278 # distance offset.
prince_pt=[954.935281491,558.241833346] #pricipal offset

camera_matrix = [[fx,   0.00000000, prince_pt[0]], 
                 [  0.00000000, fy, prince_pt[1]],
                 [  0.00000000,   0.00000000,   1.00000000]]


dist_coeffs =   [[ 0.068732817,
                  0.139845334,
                  -0.11738526,
                  -0.001002139,
                   -0.001268871]]

roll = 0.0271091714
pitch =0.0188595763
yaw = 3.137739087
YPR=[roll,pitch,yaw]

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

print ("roll = ", roll)
print ("pitch = ", pitch)
print ("yaw = ", yaw)
print( "")


def eulerAnglesToRotationMatrix(theta) :

    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])



    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])

    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])


    RotationMat = np.dot(R_z, np.dot( R_y, R_x ))

    return  RotationMat

R=eulerAnglesToRotationMatrix(YPR)


print ("Rotation matrix \n", R)

После получения матрицы поворота R. Пусть «I» будет координатами изображения в пикселях.«W» - мировые координаты.'C' - внутренняя матрица камеры.Затем я вычисляю мировые координаты W по C'R'I = W, где C '= инверсия внутренней матрицы камеры и R' = инверсия матрицы вращения.Моя камера находится в центре мира, поэтому я не рассматриваю матрицу перевода.

Код для вычисления мировых координат приведен ниже: 

Cam_intrisic=np.asanyarray(camera_matrix)
print("Camera Intrinsic \n",Cam_intrisic)
iCam_intrinsic=inv(Cam_intrisic)
iR=inv(R)
print("Inverse of Rotation matrix \n",iR)
print(iCam_intrinsic)
print("Inverse Camera matrix \n",iCam_intrinsic)


def worldCordinate(a,b):
    pixelPt = np.ones((3, 1))
    pixelPt[0, 0] = a
    pixelPt[1, 0] = b
    tempMat=np.matmul (iR,np.matmul(iCam_intrinsic, pixelPt))

    return tempMat

a=1080
b=0
print("Input pixel: ",a,b )
world=worldCordinate(a,b)
print("\n World test co-ordinates ", world)

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

...