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 / 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):

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