# MultiSense Q Reprojection Matrix

/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[2] / x_scale
cy = right_P[6] / y_scale
fx = right_P[0] / x_scale
fy = right_P[5] / y_scale
tx = (right_P[3] / 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[0][0] = fy * tx
q[1][1] = fx * tx
q[0][3] = -fy * cx * tx
q[1][3] = -fx * cy * tx
q[2][3] = fx * fy * tx
q[3][2] = -fy
q[3][3] = 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[1]
y_scale = float(cmv_2000) / disp.shape[0]
# 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[0]
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
```