To convert a disparity image to 3D you can use a Q matrix. Where Q is equal to:
Q = [ FyTx, 0, 0, -FyCxTx] [ 0, FxTx, 0, -FxCyTx] [ 0, 0, 0, FxFyTx] [ 0, 0, -Fy, 0]
The 3D point data (x,y,z) can then be calculated from pixel location (u,v) and disparity value (d) using this Q reproduction matrix.
[x, y, z, 1]^T = Q * [u, v, d, 1]
You can obtain all of these parameters from the right_P matrix stored on-board the MultiSense. This can be obtained in ROS via subscribing to the /multisense/calibration/raw_cam_cal topic, or queried using libMultiSense and the getImageCalibration method that is part of the communications channel members.
Note that the calibration information store inside the MultiSense is for the native device resolution. If you are running in a horizontally or vertically down sampled mode, you need to scale Fx, Fy, Cx, & Cy before creating the Q matrix. X values should be scaled by the horizontal scale factor while Y values should be scaled by the vertical scale factor.
The right_P matrix is of the form:
P_right = [Fx, 0, cx, FxTx] [ 0, Fy, cy, 0] [ 0, 0, 1, 0]
Python Example Code
import numpy as np import cv2 cmv_width = 2048 cmv_2000 = 1088 cmv_4000 = 2048 # Extract the values needed for building the Q matrix from the right P matrix def make_projection_matrix(right_P, x_scale, y_scale): cx = right_P / x_scale cy = right_P / y_scale fx = right_P / x_scale fy = right_P / y_scale tx = (right_P / x_scale) / fx q = np.float32([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]) # The formulas given here for constructing the Q matrix are # based on those included in the MultiSense ROS driver. q = fy * tx q = fx * tx q = -fy * cx * tx q = -fx * cy * tx q = fx * fy * tx q = -fy q = fy * (cx - cy) return q def reproject(right_P, disparity_file): disp = cv2.imread(disparity_file, cv2.IMREAD_ANYDEPTH).astype(np.float32) / 16.0 x_scale = cmv_width / disp.shape y_scale = float(cmv_2000) / disp.shape # If scale isn't a round number, then we're actually looking at an # image from a CMV4000, so use that height instead if y_scale % 1 > 0.02: y_scale = float(cmv_4000) / disp.shape q = make_projection_matrix(right_P, x_scale, y_scale) points = cv2.reprojectImageTo3D(disp, q) # # Perform additional filtering or trimming of the XYZ point cloud here # return points