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