Reproject Disparity Images to 3D Point Clouds

Disparity images can be converted to 3D Point Clouds using the factory camera calibration from the MultiSense

The OpenCV function cv::reprojectImageTo3d can be used to convert Disparity images to 3D points. This is done using the 4x4 perspective transformation Q matrix. The Q matrix can be assembled from the stereo calibration’s P Matrix.

The below equation represents a general definition for a Q Matrix for Non-Isotropic Pixels.

\[ \begin{align}\begin{aligned}\begin{split}Q = \begin{bmatrix} f_yT_x & 0 & 0 & -f_yc_xT_x \\ 0 & f_xT_x & 0 & -f_xc_yT_x \\ 0 & 0 & 0 & f_xf_yT_x \\ 0 & 0 & -f_y & f_y(c_x-c'_x) \end{bmatrix}\end{split}\\ \text{}\\f_x = \text{x focal length from the left P matrix}\\f_y = \text{y focal length from the left P matrix}\\c_x = \text{The x central pixel from the left P matrix}\\c_y = \text{The y central pixel from the left P matrix}\\Tx = \text{The x baseline from the left P matrix}\\c'_x = \text{The x central pixel from the right P matrix}\end{aligned}\end{align} \]

Note

Disparity images from the MultiSense are transmitted quantized to 1/16th of a pixel and stored with 16bit integers. Before converting a disparity image to a 3D point cloud, the disparity image need to be converted into a floating point pixel representation

C++

cv::Mat point_cloud;
cv::reprojectImageTo3D(disparity, point_cloud, Q);

Python

point_cloud = cv2.reprojectImageTo3D(disparity, Q)

Point Clouds with Intensity

The disparity image maps 1-1 with the left luma rectified image. The PointCloudUtility provides and example of how to use the left rectified image to overlay intensity data from the left luma rectified image data onto a 3D point cloud without using any 3rd party dependencies like OpenCV.

Convert Disparity Images to Depth Images

A disparity image can be converted to a depth image using the relationship between the MultiSense camera’s baseline and rectified focal length.

The following depth formula can be evaluated for every disparity pixel using \(f_x\) and \(T_x\) from the stereo calibration’s extrinsic P Matrix.

\[depth = \frac{f_x * T_x}{disparity}\]

Note

Disparity images from the MultiSense are transmitted quantized to 1/16th of a pixel and stored with 16bit integers. Before converting a disparity image to a depth image, the disparity image need to be converted into a floating point pixel representation

This operation is equivalent to taking only the z values from a reprojected 3D point cloud

Note

Invalid disparity values of 0 should be ignored when converting a dispairty image to a depth image. Depth for disparity values of 0 is undefined.

Create Color Images

The chroma image contains the CbCr channels of the planar YCbCr420 compression scheme. A RGB color image can be created using the corresponding luma image.

The OpenCV function cv::cvtColorTwoPlane can be used to convert corresponding luma and chroma images to BGR images:

C++

// Convert from YUV to BGR color space
cv::Mat bgr;
cv::cvtColorTwoPlane(luma, chroma, bgr, cv::COLOR_YUV2BGR_NV12);

Python

// Convert from YUV to BGR color space
bgr = cv2.cvtColorTwoPlane(luma, chroma, cv2.COLOR_YUV2BGR_NV12)

You can reference the LibMultiSense ColorImageUtility for an example of how to convert a luma and chroma image pair into a RGB color image without any 3rd party dependencies.

Create a Color 3D Point Cloud

Each MultiSense’s main left/right stereo pair consists of two mono camera. This configuration was chosen to optimize stereo performance over a variety of environmental conditions. Since many applications require color images, certain models of Stereo Cameras (MultiSense S27, MultiSense S30) include a third wide angle aux color camera. All 3 camera have their intrinsics and extrinsics calibrated which allows for 3D points computed from the main stereo disparity can be colorized using the data from the color aux camera.

To colorize a point cloud computed from the MultiSense disparity image, each 3D point must be projected into the aux rectified color image using the aux extrinsic projection matrix.

\[\begin{split}\begin{bmatrix} \beta * u_{aux} \\ \beta * v_{aux} \\ \beta \end{bmatrix} = P_{aux} * \begin{bmatrix} x \\ y \\ z \end{bmatrix}\end{split}\]

The LibMultiSense DepthImageUtility provides a dependency-free example of how to project 3D points computed from the disparity image into the aux rectified image for colorization.

Approximation For Execution Speed

In practice both \(T_y\) and \(T_z\) translations in the aux camera extrinsic calibration are very small (less than 1mm). By assuming both values are zero, the corresponding aux color pixel can be determined using the ratio of the aux stereo baseline and the primary stereo baseline.

\[ \begin{align}\begin{aligned}u_{aux} = u_{left} - \frac{T_{x_{aux}}}{T_{x_{right}}} * disparity\\v_{aux} = v_{left}\end{aligned}\end{align} \]

Decode a Compressed Image to RGB

The following sections provide examples of how to convert compressed I-Frames streamed via compressed image topics into RGB images.

Decode Compressed Image Streams Examples

C++
Python

Rectify Images Using OpenCV

Note

S27, S30, and KS21 cameras support onboard rectification of all image streams including aux camera sources. It’s reccomended you use these rectified streams instead of rectifying with a userspace application

Raw distorted images from the MultiSense can be unrectified using the stereo calibration parameters stored on the camera. The following examples use built-in OpenCV functions to generate rectification lookup tables used to remap raw distorted images to rectified images.

Note

The fx, fy, cx, and cy members in the M and P calibration matrices will need to be scaled based on the camera’s operating resolution before generating OpenCV rectification lookup tables

Rectify Images Example