how to match rgb image pixels with corresponding pointcloud points

Question

I have a color image, corresponding point cloud captured by oak-D camera(see the image below) and i want to get the information of pixels in the color image and corresponding point cloud value in point cloud. how can i get this information? for instance, i have a pixel value (200,250) in the color image and how to know the corresponding point value in the point cloud? any help would be appreciated.

Answer 1

It sounds like you want to project a 2D image to a 3D point cloud using the computed disparity map. To do this you will also need to know about your camera intrinsics. Since you are using the oak-D, you should be able to get everything you need with the following piece of code.

with dai.Device(pipeline) as device:
    calibData = device.readCalibration()
    
    # get right intrinsic matrix
    w, h = monoRight.getResolutionSize()
    K_right = calibData.getCameraIntrinsics(dai.CameraBoardSocket.RIGHT, dai.Size2f(w, h))

    # get left intrinsic matrix
    w, h = monoLeft.getResolutionSize()
    K_left = calibData.getCameraIntrinsics(dai.CameraBoardSocket.LEFT, dai.Size2f(w, h))
    
    R_left = calibData.getStereoLeftRectificationRotation()
    R_right = calibData.getStereoRightRectificationRotation()
    x_baseline = calibData.getBaselineDistance()

Once you have all you camera parameters, you should be able to use opencv to approach this.

First you will need to construct the Q matrix (or rectified transformation matrix).

You will need to provide

• The left and right intrinsic calibration matrices
• The Translation vector from the coordinate system of the first camera to the second camera
• The Rotation matrix from the coordinate system of the first camera to the second camera

Here's a coded example:

import numpy as np
import cv2

Q = np.zeros((4,4))
cv2.stereoRectify(cameraMatrix1=K_left,  # left intrinsic matrix
                  cameraMatrix2=K_right, # right intrinsic matrix
                  distCoeffs1=0,  
                  distCoeffs2=0,  
                  imageSize=imageSize, # pass in the image size 
                  R=R_left,            # Rotation matrix from camera 1 to camera 2
                  T=x_baseline,        # Translation matrix from camera 1 to camera 2          
                  R1=None, 
                  R2=None, 
                  P1= None, 
                  P2= None, 
                  Q=Q);

Next you will need to reproject the image to 3D, using the known disparity map and the Q matrix. The operation is illustrated below, but opencv makes this much easier.

xyz = cv2.reprojectImageTo3D(disparity, Q)

This will give you an array of 3D points. This array specifically has the shape: (rows, columns, 3), where the 3 corresponds the (x,y,z) coordinate of the point cloud. Now you can use the a pixel location to index into xyz and find it's corresponding (x, y, z) point.

pix_row = 200
pix_col = 250
point_cloud_coordinate = xyz[pix_row, pix_col, :]

See the docs for more details

• cv2.stereoRectify()
• cv2.reprojectImageTo3D()