Camera not focussing correctly

WouterOddBot

Hi,

We're running the following pipeline on a OAK-D-S2-PoE. The camera is part of a robot that moves at max 16 cm/s on an uneven surface. It is hanging about 70 cms above the ground looking straight down. Our problem is that we often get blurry images. Not motion blur, but unsharp images. Sometimes even when the robot is standing still and no vibrations either. The camera seems to have trouble focusing.

pipeline = Pipeline()
pipeline.setCameraTuningBlobPath('path/to/tuning_exp_limit_500us.bin')
color = pipeline.createColorCamera()
color.setBoardSocket(CameraBoardSocket.RGB)
color.setFps(5.0)
color.setResolution(ColorCameraProperties.SensorResolution.THE_4_K)
color.setVideoSize(2144, 2144)
color.setInterleaved(False)
color.initialControl.setSharpness(0)
color.initialControl.setLumaDenoise(0)
color.initialControl.setChromaDenoise(1)
color.initialControl.setAutoFocusLensRange(0, 255)
color_width, color_height = color.getResolutionSize()
color_auto_focus_region = self.__compute_auto_focus_region(color_width, color_height, 0.1, 0.6)
color.initialControl.setAutoFocusRegion(*color_auto_focus_region)
left = pipeline.createMonoCamera()
left.setBoardSocket(CameraBoardSocket.LEFT)
left.setFps(10.0)
left.setResolution(MonoCameraProperties.SensorResolution.THE_400_P)
right = pipeline.createMonoCamera()
right.setBoardSocket(CameraBoardSocket.RIGHT)
right.setFps(10.0)
right.setResolution(MonoCameraProperties.SensorResolution.THE_400_P)
depth = pipeline.createStereoDepth()
depth.setLeftRightCheck(True)
depth.initialConfig.setConfidenceThreshold(180)
depth.initialConfig.setMedianFilter(StereoDepthProperties.MedianFilter.MEDIAN_OFF)
depth.initialConfig.setSubpixel(True)
depth.initialConfig.setBilateralFilterSigma(0)
depth.initialConfig.setSubpixelFractionalBits(5)
depth.initialConfig.setDisparityShift(30)
depth.initialConfig.setDepthUnit(RawStereoDepthConfig.AlgorithmControl.DepthUnit.CUSTOM)
depth_config = depth.initialConfig.get()
depth_config.algorithmControl.customDepthUnitMultiplier = 50000
depth.initialConfig.set(depth_config)
depth.enableDistortionCorrection(True)
# other nodes in the pipeline that should not be relevant
# camera parameters in this code snippet are hardcoded for readability

with
def __compute_auto_focus_region(self, width: int, height: int, relative_region_width: float, relative_region_height: float) -> Tuple[int, int, int, int]:
region_width = int(width * relative_region_width)
region_height = int(height * relative_region_height)
region_width_offset = (width - region_width) // 2
region_height_offset = (height - region_height) // 2
return region_width_offset, region_height_offset, region_width, region_height

Especially the middle part of the view (from top to bottom) should be in focus, so the auto focus region is a vertical strip in the middle of the view. But the camera doesn't focus properly without setting this region either.

We also tried restricting the auto focus range with color.initialControl.setAutoFocusLensRange(120, 160), but that didn't work so well either. Could you give us a clue why the camera is not focussing as it should? Thanks in advance.

jakaskerl

WouterOddBot pipeline.setCameraTuningBlobPath('path/to/tuning_exp_limit_500us.bin')

Does it help if you remove the tuning blob? It might interfere with the autofocus functionality.

Is the focus changing a lot? Perhaps locking it to a specified value would solve the unfocusing issue?

Thanks,
Jaka

WouterOddBot

jakaskerl Hi Jaka, ok thanks we'll have a look at replacing the tuning blob with color.initialControl.setAutoExposureLimit(500).

We tried a specific value, but that doesn't seem to work.

jakaskerl

WouterOddBot
But does removing the custom blob fix the issue?

WouterOddBot

Nope it doesn't. 🙁 It also doesn't make a difference adding or removing color.initialControl.setAutoExposureLimit(500). And adding or removing the auto focus region doesn't help either.

At startup I see the camera trying to focus, but it doesn't try its full range, and eventually it comes up with an unsharp focus. Sometimes after a few minutes it suddenly gets the right focus, but often not.

WouterOddBot

Not quite sure, but perhaps it makes a difference in what focus the camera was shutdown. If the camera found the right focus, it's shutdown and started again, then it seems to find the right focus again.

jakaskerl

Hi @WouterOddBot
Can you prepare a short MRE of this so we can test it locally? It's likely a FW issue.

Thanks,
Jaka

WouterOddBot

Hi @jakaskerl , ok yes I'll try to prepare one this week.

WouterOddBot

Hi @jakaskerl ,

Just tried to upload an MRE, but it says Uploading files of this type is not allowed. Can I send it to you in another way?

I did attach an (unsharp) image from our camera. The camera is hanging about 60cm above the ground, looking down on a cardboard ridge that's about 12cm high (with a few things on top). We use pretty strong led lights for lighting. This is just a simple test setup, but still somewhat similar to the real setup in our robots. The main difference is that the robots are moving, but only at a very slow pace.

Sometimes the camera focuses correctly after about 10 secs, sometimes only after a minute or so, and sometimes it never reaches a correct focus. It's rather difficult to predict.

And while I was working on this, I noticed that the camera zooms in and out quite a bit. So the camera image changes significantly, the position of objects in the camera image also changes significantly, and the position of detection bounding boxes changes significantly as well. But the camera intrinsics and distortion coefficients don't change.

That has consequences for our real world position estimates. I noticed that our pipeline actually computes different real world positions when the camera focus changes. That of course should not happen. How can we make sure that the real world positions remain constant when the camera changes focus?

Thanks in advance!

jakaskerl

Hi @WouterOddBot
You can enclose the MRE in ``` to make it a code block.
If you have multiple files, upload it to drive or similar.

Thanks,
Jaka

WouterOddBot

from pathlib import Path

import cv2
from depthai import CameraBoardSocket
from depthai import ColorCameraProperties
from depthai import Device
from depthai import IMUSensor
from depthai import ImgFrame
from depthai import MonoCameraProperties
from depthai import Pipeline
from depthai import RawStereoDepthConfig
from depthai import StereoDepthProperties
from depthai import VideoEncoderProperties


HI_RES_STREAM_NAME = "hi_res"
COLOR_STREAM_NAME = "color"
CONTROL_STREAM_NAME = "control"
RIGHT_STREAM_NAME = "right"
DEPTH_COLOR_STREAM_NAME = "depth_color_aligned"
DEPTH_RIGHT_STREAM_NAME = "depth_right_aligned"
FEATURES_STREAM_NAME = "features"
DETECTIONS_STREAM_NAME = "detections"
IMU_STREAM_NAME = "imu"
HI_RES_IMAGE_SCRIPT_ISTREAM = "IN"
HI_RES_IMAGE_SCRIPT_OSTREAM = "OUT"


def create_hi_res_image_script(istream: str, ostream: str, modulo: int) -> str:
    return f"""
    i = 0
    while True:
        image = node.io["{istream}"].get()
        i += 1
        i %= {modulo}
        if i == 0:
            node.io["{ostream}"].send(image)
    """


pipeline = Pipeline()

color = pipeline.createColorCamera()
color.setBoardSocket(CameraBoardSocket.RGB)
color.setFps(5.0)
color.setResolution(ColorCameraProperties.SensorResolution.THE_4_K)
color.setVideoSize(2144, 2144)
color.setInterleaved(False)
color.initialControl.setSharpness(0)
color.initialControl.setLumaDenoise(0)
color.initialControl.setChromaDenoise(1)
color.initialControl.setAutoFocusLensRange(0, 255)
color.initialControl.setAutoExposureLimit(1000)

left = pipeline.createMonoCamera()
left.setBoardSocket(CameraBoardSocket.LEFT)
left.setFps(10.0)
left.setResolution(MonoCameraProperties.SensorResolution.THE_400_P)

right = pipeline.createMonoCamera()
right.setBoardSocket(CameraBoardSocket.RIGHT)
right.setFps(10.0)
right.setResolution(MonoCameraProperties.SensorResolution.THE_400_P)

depth = pipeline.createStereoDepth()
depth.setLeftRightCheck(True)
depth.initialConfig.setConfidenceThreshold(180)
depth.initialConfig.setMedianFilter(StereoDepthProperties.MedianFilter.MEDIAN_OFF)
depth.initialConfig.setSubpixel(True)
depth.initialConfig.setBilateralFilterSigma(0)
depth.initialConfig.setSubpixelFractionalBits(5)
depth.initialConfig.setDisparityShift(30)
depth.initialConfig.setDepthUnit(RawStereoDepthConfig.AlgorithmControl.DepthUnit.CUSTOM)
depth_config = depth.initialConfig.get()
depth_config.algorithmControl.customDepthUnitMultiplier = 50000
depth.initialConfig.set(depth_config)
depth.enableDistortionCorrection(True)

manip = pipeline.createImageManip()
manip.initialConfig.setResize(448, 448)
manip.initialConfig.setFrameType(ImgFrame.Type.BGR888p)

hi_res_image_script = pipeline.createScript()
hi_res_image_script.setScript(create_hi_res_image_script(HI_RES_IMAGE_SCRIPT_ISTREAM, HI_RES_IMAGE_SCRIPT_OSTREAM, 40))

video_encoder = pipeline.createVideoEncoder()
video_encoder.setProfile(VideoEncoderProperties.Profile.MJPEG)
video_encoder.setQuality(90)

features = pipeline.createFeatureTracker()
features.setHardwareResources(numShaves=2, numMemorySlices=2)

detection = pipeline.createYoloDetectionNetwork()
detection.setBlobPath(Path.home() / 'path' / 'to' / 'blob')
detection.setConfidenceThreshold(0.1)
detection.setNumClasses(2)
detection.setCoordinateSize(4)
detection.setIouThreshold(0.5)
detection.setNumInferenceThreads(2)
detection.input.setBlocking(False)
detection.input.setQueueSize(1)

imu = pipeline.createIMU()
imu.enableIMUSensor(IMUSensor.GYROSCOPE_RAW, 50)
imu.enableIMUSensor(IMUSensor.ACCELEROMETER_RAW, 50)
imu.setBatchReportThreshold(1)
imu.setMaxBatchReports(10)

control_in = pipeline.createXLinkIn()
hi_res_out = pipeline.createXLinkOut()
color_out = pipeline.createXLinkOut()
right_out = pipeline.createXLinkOut()
depth_out = pipeline.createXLinkOut()
features_out = pipeline.createXLinkOut()
detection_out = pipeline.createXLinkOut()
imu_out = pipeline.createXLinkOut()

control_in.setStreamName(CONTROL_STREAM_NAME)
hi_res_out.setStreamName(HI_RES_STREAM_NAME)
color_out.setStreamName(COLOR_STREAM_NAME)
right_out.setStreamName(RIGHT_STREAM_NAME)
depth_out.setStreamName(DEPTH_RIGHT_STREAM_NAME)
features_out.setStreamName(FEATURES_STREAM_NAME)
detection_out.setStreamName(DETECTIONS_STREAM_NAME)
imu_out.setStreamName(IMU_STREAM_NAME)

control_in.out.link(color.inputControl)
color.video.link(manip.inputImage)
color.video.link(hi_res_image_script.inputs[HI_RES_IMAGE_SCRIPT_ISTREAM])
manip.out.link(color_out.input)
manip.out.link(detection.input)
hi_res_image_script.outputs[HI_RES_IMAGE_SCRIPT_OSTREAM].link(video_encoder.input)
video_encoder.bitstream.link(hi_res_out.input)
left.out.link(depth.left)
right.out.link(depth.right)
depth.depth.link(depth_out.input)
depth.rectifiedRight.link(features.inputImage)
features.outputFeatures.link(features_out.input)
detection.out.link(detection_out.input)
depth.rectifiedRight.link(right_out.input)
imu.out.link(imu_out.input)

depth_out.input.setBlocking(False)
depth_out.input.setQueueSize(1)


#


with Device(pipeline) as device:
    color_queue = device.getOutputQueue(name=COLOR_STREAM_NAME, maxSize=4, blocking=False)
    while True:
        color_frame = color_queue.get().getCvFrame()
        print(str(device.readCalibration().getCameraIntrinsics(CameraBoardSocket.RGB)))
        print(str(device.readCalibration().getDistortionCoefficients(CameraBoardSocket.RGB)))
        cv2.imshow('color', color_frame)
        cv2.waitKey(100)

jakaskerl

WouterOddBot
I can repro your issue and it seems to me that it only happens if I use the setAutoFocusLensRange. Removing that fixes the issue. Can you confirm so I can pass it to the FW team?

Thanks,
Jaka

WouterOddBot

Hi @jakaskerl ,
Yeah it looks like it. But I'm not entirely sure, as we actually started using setAutoFocusLensRange to see if it would help solving the focus problem…

WouterOddBot

Apart from that, could you also explain a bit about my second question that the camera intrinsics don't change if the camera changes focus. I'd expect the camera intrinsics to change of the camera zooms in or out. Is that correct?

jakaskerl

WouterOddBot I'd expect the camera intrinsics to change of the camera zooms in or out. Is that correct?

We don't currently change the intrinsics; but you are correct, they change since focal length also changes.
The lens position at which camera is calibrated is written in the eeprom.

Thanks,
Jaka

WouterOddBot

Hi @jakaskerl ,

We had setAutoFocusLensRange removed now for a while, but the camera still takes quite a bit of time to focus. Not sure if it's working as it should. How much time should the camera need to focus?

Kind regards,

Wouter

WouterOddBot

Concerning the lens focus and camera intrinsics, how can we compute the correct camera intrinsics given a certain lens position? This really is important to us. If needed I can put that question in a separate topic too.

jakaskerl

WouterOddBot
You can use OPENCV's calibration procedure to only calibrate the RGB camera.

manually set the lens position to the desired one
perform calibration using the charuco board and script from opencv
you get intrinsics for that lens position
repeat for few others

We are working on fitting a function to the lens position <-> intrinsics relationship. You can wait for the release or do it yourself.

Thanks,
Jaka

WouterOddBot

Ha thanks great that you're already working on it. We'll wait for the release.

Joshua

@jakaskerl Thank you for this helpful thread! Is the lens position <-> intrinsics function available yet for the OAK-D pro poe autofocus camera? It'd be really helpful!

jakaskerl

Joshua
It's not available because the lenses are not always mounted in the same way due to manufacturing imperfections. We will need a way to interpolate for this information, we are working on it.

Thanks,
Jaka