OAK ToF minimizing RGB and ToF motion blur

Wen

I would like to live stream OAK ToF camera and save undistorted, spatially aligned and time synced RGB image, depth image, and point cloud using the newest DepthAI. Due to my sensor application requirements, I must minimize motion blur for both RGB and ToF cameras as much as possible, as the camera will be mounted on a fast-moving vehicle. This is the code that I have currently based on the RGBD node:

import depthai as dai

import cv2

import numpy as np

import open3d as o3d

import os

output_dir = "oak_tof_capture"

os.makedirs(output_dir, exist_ok=True)

with dai.Pipeline() as pipeline:

rgbd = pipeline.create(dai.node.RGBD).build(

    autocreate=True,

    mode=dai.node.StereoDepth.PresetMode.DEFAULT,

    size=(640, 400)

)

tof = next(n for n in pipeline.getAllNodes() if isinstance(n, dai.node.ToF))

config = tof.getInitialConfig()

config.enableOpticalCorrection = True

config.enableDistortionCorrection = True

config.enablePhaseShuffleTemporalFilter = False

config.enablePhaseUnwrapping = False

config.phaseUnwrappingLevel = 0

config.phaseUnwrapErrorThreshold = 300

config.enableBurstMode = True

config.setMedianFilter(dai.MedianFilter.KERNEL_5x5)

tof.setInitialConfig(config)

cam_rgb = next(n for n in pipeline.getAllNodes() if isinstance(n, dai.node.Camera))

camControlQueue = cam_rgb.inputControl.createInputQueue()

qRgbd = rgbd.rgbd.createOutputQueue(maxSize=1, blocking=False)

qPcl  = rgbd.pcl.createOutputQueue(maxSize=1, blocking=False)

pipeline.start()

ctrl = dai.CameraControl()

ctrl.setAutoExposureLimit(500)

camControlQueue.send(ctrl)

frame_id = 0

while pipeline.isRunning():

    rgbdData = qRgbd.get()

    pclData  = qPcl.get()

    rgb   = rgbdData.getRGBFrame().getCvFrame()

    depth = rgbdData.getDepthFrame().getCvFrame()

    depth_vis = cv2.normalize(depth, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8)

    cv2.imshow("RGB", rgb)

    cv2.imshow("Depth", depth_vis)

    cv2.imwrite(f"{output_dir}/rgb_{frame_id:06d}.png", rgb)

    cv2.imwrite(f"{output_dir}/depth_{frame_id:06d}.png", depth)

    points, colors = pclData.getPointsRGB()

    pcd = o3d.geometry.PointCloud()

    pcd.points = o3d.utility.Vector3dVector(points.astype(np.float64))

    colors_rgb = np.delete(colors, 3, 1).astype(np.float64) / 255.0

    pcd.colors = o3d.utility.Vector3dVector(colors_rgb)

    o3d.io.write_point_cloud(f"{output_dir}/pc_{frame_id:06d}.ply", pcd)

    frame_id += 1

    if cv2.waitKey(1) == 27:

        break

I was able to control motion blur for RGB camera using setAutoExposureLimit. I configured the ToF camera based on this link: https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/tof/. Unfortunately, my depth images and point clouds are still rather noisy even when the camera moves slightly. I have a few questions:

1. rgbd = pipeline.create(dai.node.RGBD).build(

    autocreate=True,

    mode=dai.node.StereoDepth.PresetMode.DEFAULT,

    size=(640, 400)

)

Here the depth and point cloud information comes from the stereo pair or the ToF camera? I would like to use the ToF camera.

2. Is there anything that can be optimized in the code?

3. According to the link, "Sensor/emitter can go up to 160 FPS, which will translte to depth output at either 40 FPS (burst mode enabled) or 80 FPS (burst mode disabled). This is due to different modulation frequencies (80MHz and 100MHz) and the need to combine shuffled/non-shuffled frames to reduce noise." How can I set the frame rate of the ToF camera explicitly to the absolute max? I can't seem to figure that out.

OskarSonc

Hey @Wen - thanks for sharing this, your current RGBD.build(autocreate=True, ...) will use the ToF depth stream when a ToF sensor is detected on the device, and only fall back to stereo when ToF is not available. The main reason you still see noisy depth while moving is that your current ToF settings disable some of the built-in stabilization/noise-reduction behavior, so I’d recommend returning to the default phase settings first and then tuning from there, plus adding a ToF confidence filter to reject low-confidence pixels before point cloud export. For frame rate, you need to set ToF FPS explicitly (or pass FPS through RGBD build when using autocreate). The practical maximum is typically around 80 depth FPS with burst mode disabled, or around 40 depth FPS with burst mode enabled, with burst giving better robustness to motion artifacts. Also, setAutoExposureLimit affects RGB only. ToF usually stays at its default rate (commonly around 30 FPS) unless FPS is explicitly set in ToF.build(...) or RGBD.build(...).
Also take a look at this thread.

Thanks,
Oskar

Wen

I inspected the point clouds saved by the code. I noticed that the point clouds do not accurately represent the geometries of the scanned surfaces. It seems like the enableDistortionCorrection was not working properly. For example I took a measurement of a flat floor, the resulting point cloud was not flat at all:

I tried to measure many other flat surfaces and observed the exact same thing. How can this be fixed?

OskarSonc

Thanks for sharing the screenshot, this looks more like ToF phase/range behavior than a distortion-correction bug. In your current config, disabling enablePhaseUnwrapping and enablePhaseShuffleTemporalFilter can make flat surfaces look curved/noisy, and with unwrapping off the unambiguous range is limited, which can also warp floor geometry. I recommend restoring default ToF phase settings first, then tuning burst mode and adding confidence filtering for low-confidence pixels. We just updated ToF node page that now has much more info.

Wen

OskarSonc

After numerous trials, I found only when enablePhaseShuffleTemporalFilter and enableBurstMode are both False, the point clouds and depth images can occasionally be flat for a flat surface, but still most of the time the data were wavy or distorted. For any other settings (both True, one True one False), the point clouds or depth images were always wavy for a flat surface. Since my camera is very close to the sensing surface (20-30 cm distance), I need to set phaseUnwrappingLevel as 0, which is equivalent to disabling enablePhaseUnwrapping anyways. I do not believe the confidence filtering would be helpful in my case, as the entire point cloud surface or depth image surface is wavy, instead of only a small region being wavy that can be filtered out. Are there alternative solutions I can try? Thanks.

OskarSonc

Hm I see, @DavidZah any ideas?

DavidZah

Hi Wen,

First, I apologize for the late response!

To help us isolate the issue, could you please take a measurement at a distance of, for example, 1 meter from a flat wall and let us know if the waviness persists?

Since you are operating very close to the surface (20-30 cm), the waviness you are seeing could be related to "wiggle error" (a cyclic error common in ToF sensors that can warp flat surfaces ). You can try disabling the wiggle correction in the ToF base node to see if it improves your close-range results. In your Python config, you can add:

config.enableWiggleCorrection = False

Additionally, just to clarify—are you currently using DepthAI v2 or v3?