ToF Measurement Error

TTom · Sep 12, 2024

Hello,

I am having some problems similar to the original post in this link, but didn't want to derail the current discussion so I'm making this post. My ToF sensor is outputting values that are not aligned well with reality. See some measurements I took with a measuring tape vs the ToF sensor below.

I am using an OAK FFC 4P board with an IMX577 on CAM_A and the ToF on CAM_C. I have calibrated the pair several times, and tried different ToF modules, and get the same rough results always. My calibration had good error, and is attached at the bottom of this post. My depthai version is 2.28.0.0.dev0+76294530fd0bf91b09469490a1d14871c374630a.

Any pointers or things to try would be appreciated! Thanks.

{
    "batchName": "",
    "batchTime": 1700737592,
    "boardConf": "IR-C00M00-00",
    "boardCustom": "",
    "boardName": "DD2090",
    "boardOptions": 0,
    "boardRev": "R7M1E7",
    "cameraData": [
        [
            0,
            {
                "cameraType": 0,
                "distortionCoeff": [
                    13.725380897521973,
                    -27.1486873626709,
                    0.00014784393715672195,
                    0.00025289796758443117,
                    56.11801528930664,
                    13.626705169677734,
                    -26.974199295043945,
                    56.287872314453125,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0
                ],
                "extrinsics": {
                    "rotationMatrix": [
                        [
                            0.0,
                            0.0,
                            0.0
                        ],
                        [
                            0.0,
                            0.0,
                            0.0
                        ],
                        [
                            0.0,
                            0.0,
                            0.0
                        ]
                    ],
                    "specTranslation": {
                        "x": -0.0,
                        "y": -0.0,
                        "z": -0.0
                    },
                    "toCameraSocket": -1,
                    "translation": {
                        "x": 0.0,
                        "y": 0.0,
                        "z": 0.0
                    }
                },
                "height": 3040,
                "intrinsicMatrix": [
                    [
                        2526.64208984375,
                        0.0,
                        2089.663330078125
                    ],
                    [
                        0.0,
                        2524.888427734375,
                        1464.1832275390625
                    ],
                    [
                        0.0,
                        0.0,
                        1.0
                    ]
                ],
                "lensPosition": 0,
                "specHfovDeg": 75.0,
                "width": 4056
            }
        ],
        [
            2,
            {
                "cameraType": 0,
                "distortionCoeff": [
                    -9.26715087890625,
                    29.49131202697754,
                    -0.00011109501792816445,
                    -0.0006627050461247563,
                    -15.05420207977295,
                    -9.26780891418457,
                    29.405651092529297,
                    -14.602873802185059,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0
                ],
                "extrinsics": {
                    "rotationMatrix": [
                        [
                            0.9998121857643127,
                            -0.01154649443924427,
                            -0.015566435642540455
                        ],
                        [
                            0.011661147698760033,
                            0.9999054074287415,
                            0.007294856943190098
                        ],
                        [
                            0.015480732545256615,
                            -0.007475009188055992,
                            0.9998522400856018
                        ]
                    ],
                    "specTranslation": {
                        "x": -1.996000051498413,
                        "y": 0.0,
                        "z": 0.0
                    },
                    "toCameraSocket": 0,
                    "translation": {
                        "x": -3.045868158340454,
                        "y": -0.02484941855072975,
                        "z": -1.2451037168502808
                    }
                },
                "height": 480,
                "intrinsicMatrix": [
                    [
                        468.7503662109375,
                        0.0,
                        319.3566589355469
                    ],
                    [
                        0.0,
                        468.7717590332031,
                        234.0368194580078
                    ],
                    [
                        0.0,
                        0.0,
                        1.0
                    ]
                ],
                "lensPosition": 0,
                "specHfovDeg": 70.0,
                "width": 640
            }
        ]
    ],
    "deviceName": "",
    "hardwareConf": "F0-FV00-BC000",
    "housingExtrinsics": {
        "rotationMatrix": [],
        "specTranslation": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        },
        "toCameraSocket": -1,
        "translation": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        }
    },
    "imuExtrinsics": {
        "rotationMatrix": [
            [
                0.0,
                0.0,
                0.0
            ],
            [
                0.0,
                0.0,
                0.0
            ],
            [
                0.0,
                0.0,
                0.0
            ]
        ],
        "specTranslation": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        },
        "toCameraSocket": -1,
        "translation": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        }
    },
    "miscellaneousData": [],
    "productName": "OAK-FFC-4P",
    "stereoEnableDistortionCorrection": false,
    "stereoRectificationData": {
        "leftCameraSocket": 0,
        "rectifiedRotationLeft": [
            [
                0.925619900226593,
                0.00755158020183444,
                0.37837907671928406
            ],
            [
                -0.006574939005076885,
                0.9999709129333496,
                -0.0038730097003281116
            ],
            [
                -0.3783973157405853,
                0.0010971155716106296,
                0.9256426095962524
            ]
        ],
        "rectifiedRotationRight": [
            [
                0.931391716003418,
                -0.00596518674865365,
                0.3639696538448334
            ],
            [
                0.005027147009968758,
                0.999981164932251,
                0.003524555591866374
            ],
            [
                -0.36398380994796753,
                -0.001453012810088694,
                0.9314041137695313
            ]
        ],
        "rightCameraSocket": 2
    },
    "stereoUseSpecTranslation": true,
    "version": 7,
    "verticalCameraSocket": -1
}

jakaskerl · Sep 13, 2024

Tom
Calibration might be missing from your TOF modules (if they were bought before 24 March 2024 or are from a batch before that). A calibration won't fix this as intrinsics are written on module eeprom and are accessible with calibraiton script.

If that is the case, send email to support@luxonis.com so we can replace the module with a calibrated one.

Thanks,
Jaka

TTom · Sep 13, 2024

jakaskerl

I purchased these recently a few weeks ago, so I don't think that is the case.

jakaskerl · Sep 15, 2024

Tom
Please run a script with DEPTHAI_LEVEL=info env variable and paste the ToF module serial number (ID) here so we can check if the calibration is present in the logs.

Thanks,
Jaka

TTom · Sep 16, 2024

jakaskerl

This module is: f1268b911b81d286fb0ea58a363f8036

We also have another module, a536a23694e05efff4cb49a8175e5257.

CenekAlbl · Sep 17, 2024

Hi Tom, sorry to hear about your issues! I am wondering if you could provide some details on how you are measuring the error? And ideally the script where you create the pipeline? Or are you simply using depthai Viewer?

TTom · Sep 17, 2024

CenekAlbl

Hello! I am setting the module up looking at a planar surface (table). I have measured the distance to be roughly 245 mm to the module's lens.

I am using the script attached below. When I get a frame, I take a region in the center (denoted by the white box), and average all the pixel readings. The reading will fluctuate and drift between 175 mm and 185 mm.

Here is the script. Just noticed that I'm really taking the readings in a rectangle and not circle, but this shouldn't change my results - I've updated the image.

"""ToF depth measurement example."""

import cv2
import depthai as dai
import numpy as np

# Data type of the ToF frame.
TOF_DTYPE = np.uint16

# Radius [px] of the region to take a depth measurement.
TOF_MEASUREMENT_RADIUS = 100

# ToF resolution.
TOF_RESOLUTION = (640, 480)


def create_pipeline() -> dai.Pipeline:
    """Create the pipeline."""
    pipeline = dai.Pipeline()

    # Create the ToF node.
    tof = pipeline.create(dai.node.ToF)
    tof_config = tof.initialConfig.get()
    tof_config.enableOpticalCorrection = True
    tof_config.enablePhaseShuffleTemporalFilter = True
    tof_config.phaseUnwrappingLevel = 0
    tof_config.enableTemperatureCorrection = True
    tof_config.enableFPPNCorrection = True
    tof.initialConfig.set(tof_config)

    # Create the camera node.
    cam_tof = pipeline.create(dai.node.Camera)
    cam_tof.setFps(60)
    cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_C)
    cam_tof.raw.link(tof.input)

    # Create the XLinkOut node.
    xout = pipeline.create(dai.node.XLinkOut)
    xout.setStreamName("depth")
    tof.depth.link(xout.input)

    return pipeline


def get_depth_measurement(depth_map: np.ndarray, radius: int) -> float:
    """Get depth measurement from the center of the depth map."""
    center_point = (depth_map.shape[1] // 2, depth_map.shape[0] // 2)
    depth_map_center = depth_map[
        center_point[1] - radius : center_point[1] + radius,
        center_point[0] - radius : center_point[0] + radius,
    ]
    return np.average(depth_map_center)


def create_colored_image(depth_map: np.ndarray, color_map: int) -> np.ndarray:
    """Create colored image from depth map."""
    depth_normalized = np.interp(
        depth_map,
        (np.percentile(depth_map, 5), np.percentile(depth_map, 95)),
        (0, 255),
    ).astype(np.uint8)
    return cv2.applyColorMap(depth_normalized, color_map)


def plot_depth_measurement(
    image: np.ndarray,
    depth_measurement: float,
    radius: int,
) -> np.ndarray:
    """Plot depth measurement and region on the image."""
    center_point = (image.shape[1] // 2, image.shape[0] // 2)
    cv2.rectangle(
        image,
        (center_point[0] - radius, center_point[1] - radius),
        (center_point[0] + radius, center_point[1] + radius),
        (255, 255, 255),
        2,
    )
    cv2.putText(
        image,
        f"Depth: {depth_measurement:.2f} mm",
        (10, 30),
        cv2.FONT_HERSHEY_SIMPLEX,
        1,
        (255, 255, 255),
        2,
        cv2.LINE_AA,
    )
    return image


def main() -> None:
    """ToF depth measurement example."""
    with dai.Device(create_pipeline()) as device:
        depth_queue = device.getOutputQueue(name="depth")
        while True:
            # Get frame.
            tof_frame = depth_queue.get().getFrame()  # type: ignore[attr-defined]
            depth_map = np.frombuffer(tof_frame, dtype=TOF_DTYPE).reshape(
                (TOF_RESOLUTION[1], TOF_RESOLUTION[0])
            )

            # Take depth measurement at center of frame.
            depth_measurement = get_depth_measurement(depth_map, TOF_MEASUREMENT_RADIUS)

            # Depth map colorization.
            image = create_colored_image(depth_map, cv2.COLORMAP_JET)

            # Plot depth measurement and region.
            image = plot_depth_measurement(
                image, depth_measurement, TOF_MEASUREMENT_RADIUS
            )

            # Display frame.
            cv2.imshow("ToF Depth", image)
            if cv2.waitKey(1) & 0xFF == ord("q"):
                cv2.destroyAllWindows()
                break


if __name__ == "__main__":
    main()

CenekAlbl · Sep 17, 2024

Thanks! I believe I know what the reason is that the closest measurement significantly more off - the LED emitor reflects strongly from close distances and saturates the sensor so the exposure (duty cycle) automatically decreases to compensate. When the exposure time decreases, the tempreature of the LED drops significantly too. This causes a phase shift actually and changes the depth measurement.

You do not have temperature compensation enabled. You can try to enable it, it should make things better. But I have to check with the team if temperature compensation was enabled for the FFC modules already and since when, whether its available for your unit.

TTom · Sep 17, 2024

CenekAlbl

Thanks for the explanation! Enabling the temperature setting does seem to help, but not quite make it accurate. With it enabled, I am getting a reading of around 195 mm instead of 175 mm.

For my use case, I will typically be in the 0.25-0.35 meter range, so this is where accuracy is most important to me.