Calculating depth from disparity map

finn

Hi,

I am trying to calculate the depth image using the disparity map, by using the formula:
z = fx * baseline / disparity

I do get my calculated depth map to look like the depth map coming from oak-d (see attached images) but there seems to be a scaling issue. Picking out a random pixel I get: disparity: 1123, depth: 1626 and depth calculated : 18.93. From reading the calibration my focal length is 283.42 and my baseline 75mm.
What am I doing wrong? Thanks

def to_cmap(frame: np.ndarray):
    max_ = np.max(frame)
    min_ = np.min(frame)
    if max_ - min_ == 0:
        return np.zeros_like(frame)
    frame = (frame - min_) / (max_ - min_) * 255
    frame = frame.astype(np.uint8)
    frame = cv2.applyColorMap(frame, cv2.COLORMAP_JET)
    return frame


def disp_2_depth(disparity: np.ndarray, baseline: float, fx :float):
    depth = np.where(disparity == 0, 0, fx * baseline / disparity)
    return depth


def main():
    print(dai.Device.getAllAvailableDevices())

    dev = dai.Device("192.168.178.120")

    calib = dev.readCalibration()
    baseline = calib.getBaselineDistance()
    baseline *= 10 # mm
    print(baseline)

    intrinsics = np.array(calib.getCameraIntrinsics(dai.CameraBoardSocket.CAM_C, IMG_WIDTH, IMG_HEIGHT))
    fx = intrinsics[0, 0].item()
    fy = intrinsics[1, 1].item()
    print(f"fx: {fx}, fy: {fy} Pixel")

    hfov = calib.getFov(dai.CameraBoardSocket.CAM_C)
    fx_calc = IMG_WIDTH * 0.5 / tan(hfov * 0.5 * pi / 180)
    print(f"focal length: {fx_calc} - using fx from intrinsics")

    pipeline = dai.Pipeline(dev)

    left_cam = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
    right_cam = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)

    right = right_cam.requestOutput((IMG_WIDTH, IMG_HEIGHT), enableUndistortion=False)
    left = left_cam.requestOutput((IMG_WIDTH, IMG_HEIGHT), enableUndistortion=False)

    stereo = pipeline.create(dai.node.StereoDepth)
    stereo.setDepthAlign(dai.CameraBoardSocket.CAM_C)
    stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)

    stereo.setSubpixel(False)
    stereo.setExtendedDisparity(False)

    print(stereo.initialConfig)

    left.link(stereo.left)
    right.link(stereo.right)

    queue = {
        "disparity": stereo.disparity.createOutputQueue(),
        "depth": stereo.depth.createOutputQueue(),
             }

    pipeline.start()

    x, y = 200, 200 # get values at some random pixels

    while True:
        disp = queue["disparity"].get()
        depth = queue["depth"].get()
        assert isinstance(disp, dai.ImgFrame)
        assert isinstance(depth, dai.ImgFrame)

        print(disp.getType(), disp.getCvFrame().dtype)

        disp = disp.getCvFrame()
        depth = depth.getCvFrame()

        disp_px = disp / 8 # Converting to px

        cv2.imshow("disp", to_cmap(disp))
        cv2.imshow("depth", to_cmap(depth))
        depth_calc = disp_2_depth(disp_px, baseline, fx)

        print(f"disp: {disp[x, y]}, disp_px: {disp_px[x, y]}, depth: {depth[x, y]}, depth calc: {depth_calc[x, y]}")
        cv2.imshow("depth calculated", to_cmap(depth_calc))
        if cv2.waitKey(1) == ord("q"):
            break

    cv2.destroyAllWindows(

Disparity (coming from oak-d)

Depth (also from oak-d):

Depth calculated:

Oskar-Luxonis

Hey finn - your formula and baseline conversion are correct. The issue is that the RAW16 disparity output is not in pixel units for this v3 configuration.

With DEFAULT, setSubpixel(False), and setExtendedDisparity(False), convert the raw disparity to pixels first:

max_raw_disparity = stereo.initialConfig.getMaxDisparity()  # 8192 in this setup
disparity_px = disp.astype(np.float32) * 95.0 / max_raw_disparity

depth_calc = np.where(
    disparity_px == 0,
    0,
    fx * baseline / disparity_px,
)

So for your example, a raw disparity of 1123 corresponds to about 13.0 px, which gives approximately 1626 mm with fx=283.42 and a 75 mm baseline.

The /8 conversion is for the usual 3-bit subpixel representation, but you explicitly disable subpixel after applying the default preset, so it does not apply here.

Thanks,
Oskar

finn

Hey @Oskar-Luxonis

Thanks, that helps. If I do activate 3-bit subpixel representation, converting my raw disparity into pixel changes to:
disp_px = disp.astype(np.float32) * 754.0 / max_disparity / 8
following this documentation. It does not quite fit the depth coming from oak-d.
Thanks, Finn

Oskar-Luxonis

Hey finn - 754 is the number of available 3-bit subpixel values, not the correct scale for the filtered RAW16 disparity output.

With your DEFAULT preset and 3-bit subpixel enabled, please use:

disp_px = disp.astype(np.float32) / 80.0

Then apply depth = fx * baseline / disp_px as before. The post-processing filters add an extra integer scale factor of 10, so /8 alone is not enough, and getMaxDisparity() returning 8192 is not the effective maximum in this case.

Thanks,
Oskar