Aligning images with the depth map in Opencv

user_8382707 0
Viewed 1040

I am working on a dataset where we have RGB images and corresponding depth maps. The depth maps aren't aligned perfectly with the RGB images and we are given the Sensor Calibration for aligning. Here are the params:

intrinsic matrix for the depth sensor
584.27 0 337.11
0 584.27 254.17
0 0 1

intrinsic matrix of the color camera
519.47 0 329.76
0 519.47 264.09
0 0 1

extrinsic parameters mapping the depth coordinate system to the camera coordinate system
rotation matrix 
0.999947 -0.00432361 0.00929419 
0.00446314 0.999877 0.0150443 
-0.009228 -0.015085 0.999844 

translation vector
25.0198  -0.2000  3.2308

How can I use these to align the depth maps with the RGB images in OpenCV python?

alignment opencv image-processing python-3.x python
1 Answers

I had the same problem, here what I've done in Python (where K is intrinsic for depth sensor and color cameras, and ext the extrinsic):

# All what we need to align our images:

depth_scale = 0.001

fx_d = K_depth[0,0]
fy_d = K_depth[1,1]
cx_d = K_depth[0,2]
cy_d = K_depth[1,2]

fx_rgb = K_col[0,0]
fy_rgb = K_col[1,1]
cx_rgb = K_col[0,2]
cy_rgb = K_col[1,2]

height = depth.shape[0]
width = depth.shape[1]

aligned = np.zeros((height,width,6))

for v in range (height):
    for u in range (width):
        # Apply Depth intrinsics
        z = depth[v,u].sum() * depth_scale
        x = ((u - cx_d) * z) / fx_d
        y = ((v - cy_d) * z) / fy_d
        
        # Apply extrinsic
        transformed = np.dot(ext ,np.array([x,y,z,1])).T
        aligned[v,u,0] = transformed[0]
        aligned[v,u,1] = transformed[1]
        aligned[v,u,2] = transformed[2]
        
for v in range (height):
    for u in range (width):
        # Apply RGB intrinsic
        x = (aligned[v,u,0] * fx_rgb / aligned[v,u,2]) + cx_rgb
        y = (aligned[v,u,1] * fy_rgb / aligned[v,u,2]) + cy_rgb
        
        # Endle out of bound pixels
        if x > width-1 or y > height-1 or x < 0 or y < 0:
            pass
        
        else:
            x = int(round(x))
            y = int(round(y))
        
            aligned[v,u,3] = color[y, x, 0]
            aligned[v,u,4] = color[y, x, 1]
            aligned[v,u,5] = color[y, x, 2]
            
# Retrive RGB value from our aligned version
test = np.zeros((height,width,3))
for i in range (height):
    for j in range (width):
        test[i,j] = aligned[i,j][3:6]
plt.imshow(test.astype(int))
user_125322180
Related