Блог

Я хочу объединить облака точек с нескольких камер глубины вместе. Для достижения этой цели я пытаюсь преобразовать облако точек из каждого кадра камеры в единый мировой кадр, определенный кодом aruco (похожим на QR-код).

Прямо сейчас я только начинаю с одной камеры.

Допустим, у меня есть

 k = #camera intrinsic matrix (3x3)
aruco_world_coords = #coordinates (meters) of the aruco code corners in the real world (4x3) 
                     e.g. [[0,    0,  0],
                           [.1,   0,  0],
                           [.1, -.1,  0],
                           [0,  -.1,  0]]
rgb_image = #camera's color image (width x height x 3)
cam_pc = #point cloud in camera's frame (nx3)
 

В настоящее время я занимаюсь следующим:

 import cv2 as cv
import numpy as np
from cv2 import aruco

#######################################################################
#####          Detect Aruco code and extract rvec and tvec       ######
#######################################################################

gray = cv.cvtColor(rgb_image, cv.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
params =  aruco.DetectorParameters_create()

corners, _, _ = aruco.detectMarkers(gray, aruco_dict, parameters=params)

rvec, tvec = cv.solvePnP(QR_world_coords, 
                            corners[0], 
                            K, None, None, None,
                            False, cv.SOLVEPNP_ITERATIVE)

rmatrix, _ = cv.Rodrigues(rvec)

#######################################################################
#####                  Calculate new point cloud                 ######
#######################################################################

new_pc = np.matmul(np.linalg.inv(rmatrix), cam_pc.T-tvec).T

#######################################################################
#####           Same math in a different way to verify result     #####
#######################################################################

'''
t = -rmatrix.T.dot(tvec)
rmatrix_ = rmatrix.T
new_pc = cam_pc.dot(rmatrix_.T)   tvec.squeeze()
'''

#######################################################################
##### Same math, in yet another way, to be super sure it's right  #####
#######################################################################

'''
m = np.hstack([rmatrix, trans])
m = np.vstack([m, np.array([[0,0,0,1]])])
m = np.linalg.inv(m)

cam_pc_homog = np.concatenate([cam_pc, np.ones((pc.shape[0],1))], axis=1)
new_pc = np.matmul(m, cam_pc_homog.T).T
new_pc /= new_pc[:,3].reshape(-1,1)
new_pc = new_pc[:,:3]
'''

 

Затем я отобразил новое облако точек в сцене с осью, нарисованной в начале координат (0,0,0).

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

Однако я также ожидаю, что облако точек теперь должно оставаться неподвижным, даже когда я поворачиваю камеру, поскольку точки были преобразованы в мировой кадр, и мир не движется относительно мирового кадра. Этого не происходит, точки продолжают двигаться (хотя и по-другому), когда я перемещаю камеру.

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

Любая помощь будет очень признательна!