Hi from Pollen Robotics 🙂 We just received our ToF module. Using this example we get a pretty nice looking depthmap (Except for the looping colors, but I assume that over a few meters the sensor is not accurate anymore) (We are using an OAK FFC-4P btw) However, when I try to mesure distances, they are pretty far off most of the time. For example Real distance (black circle) ~= 380 Real distance ~= 340 Real distance ~= 195 Real distance ~= 880 A few questions : I don't understand what the different parameters are doing, and the documentation is not very helpful to me (this is the first time I work with a ToF sensor). Could you explain what freqModUsed, avgPhaseShuffle, and minimumAmplitude are doing ? Am I right to expect millimeters out of the depth map ? Is there any calibration needed ? Could that have been degraded during transport ? Here is my code for depth mesurement in the image (at the position of the cursor), based on the provided example #!/usr/bin/env python3 import cv2 import depthai as dai import numpy as np pipeline = dai.Pipeline() cam_a = pipeline.create(dai.node.Camera) cam_a.setBoardSocket(dai.CameraBoardSocket.CAM_A) tof = pipeline.create(dai.node.ToF) # Configure the ToF node tofConfig = tof.initialConfig.get() tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MAX tofConfig.depthParams.avgPhaseShuffle = False tofConfig.depthParams.minimumAmplitude = 3.0 tof.initialConfig.set(tofConfig) cam_a.raw.link(tof.input) xout = pipeline.create(dai.node.XLinkOut) xout.setStreamName("depth") tof.depth.link(xout.input) last_mouse_pos = (0, 0) last_depth_mesurement = 0 def mouse_callback(event, x, y, flags, params): global last_mouse_pos last_mouse_pos = (x, y) cv2.namedWindow("colorized depth") cv2.setMouseCallback("colorized depth", mouse_callback) time_averaged_depth = [] # Connect to device and start pipeline with dai.Device(pipeline) as device: print('Connected cameras:', device.getConnectedCameraFeatures()) q = device.getOutputQueue(name="depth") while True: imgFrame = q.get() depth_map = imgFrame.getFrame() depth_downscaled = depth_map[::4] non_zero_depth = depth_downscaled[depth_downscaled != 0] # Remove invalid depth values if len(non_zero_depth) == 0: min_depth, max_depth = 0, 0 else: min_depth = np.percentile(non_zero_depth, 3) max_depth = np.percentile(non_zero_depth, 97) last_depth_mesurement = depth_map[last_mouse_pos[1]-5:last_mouse_pos[1]+5, last_mouse_pos[0]-5:last_mouse_pos[0]+5] last_depth_mesurement = np.mean(last_depth_mesurement) time_averaged_depth.append(last_depth_mesurement) time_averaged_depth = time_averaged_depth[20:] if len(time_averaged_depth) > 20 else time_averaged_depth print("mean :", np.mean(time_averaged_depth), "(std :", np.std(time_averaged_depth), ")") depth_colorized = np.interp(depth_map, (min_depth, max_depth), (0, 255)).astype(np.uint8) depth_colorized = cv2.applyColorMap(depth_colorized, cv2.COLORMAP_JET) depth_colorized = cv2.circle(depth_colorized, last_mouse_pos, 5, (0, 0, 0), -1) cv2.imshow("colorized depth", depth_colorized) if cv2.waitKey(1) == ord('q'): break if(window.hljsLoader && !document.currentScript.parentNode.hasAttribute('data-s9e-livepreview-onupdate')) { window.hljsLoader.highlightBlocks(document.currentScript.parentNode); } Thanks ! Antoine

Hi apirrone , We apologize for the inconvenience, but the full TOF decoding is not yet implemented, and we are working on it at the moment. It should be ready in firmware in a few week, so you'll be able to just update the library (pip install depthai -U) and have accurate depth. Kind regards, Erik

Thanks erik for the reply, please keep me updated when the update is ready! Just curious, how is it that it "kind of works" right now, but with bad accuracy? The colorized depth map looks really good and continuous 😅

Hi apirrone , That's the thing - we get some data out, but full decoding and error correction aren't implemented. The depth does look decent (at least the corners are very sharp), but if you'd visualize the pointcloud it's completely off.

Hi @erik , Any news on the firmware ? Thanks ! Antoine

Hi @apirrone , We got the decoding mostly working, there's just some offset left that we haven't figured out. Below is an A4 paper sheet and its size - about 10% dimension mismatch. We believe we'll have it ready this month. Thanks, Erik

Hi @erik , any news ? We are very eager to try it out ! 🙂 Thanks, Antoine

Hi apirrone We need to finalize a few things, I'd say roughly about 2 weeks.. Thanks, Jaka

jakaskerl hey any update on the ToF sensor finalization so its accurate and ready to be used in depthai?

ToF bad accuracy

apirrone

jakaskerl

So If I understand correctly, I should set the camera extrinsics as follows for example ? :

right->left
left->tof
tof->-1

I get an error if I set -1 or "-1" for toCameraSocket

Traceback (most recent call last):
  File "/home/antoine/Pollen/pollen-vision/pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py", line 31, in <module>
    w.flash(args.calib_json_file, tof=args.tof)
  File "/home/antoine/Pollen/pollen-vision/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py", line 312, in flash
    ch.setCameraExtrinsics(
TypeError: setCameraExtrinsics(): incompatible function arguments. The following argument types are supported:
    1. (self: depthai.CalibrationHandler, srcCameraId: depthai.CameraBoardSocket, destCameraId: depthai.CameraBoardSocket, rotationMatrix: list[list[float]], translation: list[float], specTranslation: list[float] = [0.0, 0.0, 0.0]) -> None

Invoked with: <depthai.CalibrationHandler object at 0x7e4bb400a5f0>, <CameraBoardSocket.CAM_D: 3>, '-1', [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], [-3.2509, 0.0, 0.0]; kwargs: specTranslation=[-3.2509, 0.0, 0.0]

So, should it look like this ?

ch.setCameraExtrinsics(
    left_socket,
    tof_socket,
    R_left_to_tof,
    T_left_to_tof,
    specTranslation=T_left_to_tof,
)

ch.setCameraExtrinsics(
    right_socket,
    left_socket,
    R_right_to_left,
    T_right_to_left,
    specTranslation=T_right_to_left,
)

# Should I do this ?
ch.setCameraExtrinsics(
    tof_socket,
    tof_socket,
    [0, 0, 0],
    np.eye(3).tolist(),
    specTranslation=[0, 0, 0],
)

jakaskerl

apirrone

apirrone I get an error if I set -1 or "-1" for toCameraSocket

You have to set it to CameraBoardSocket.AUTO instead of CameraBoardSocket.CAM_X. The enum value is -1.

apirrone How can I get the real intrinsic matrix of the ToF ? I guess it's flashed in it's own eeprom ?

Yes, the intrinsics are flashed in the module eeprom.

apirrone Is there an easy way to get the depth in full resolution and the relevant aligned rgb section cropped ? So that I have two 640x480 (depth and rgb) that are aligned ?

Undistort the RGB image first. More on this here.

Thanks,
Jaka

apirrone

jakaskerl Make sure the .json in properly flashed. I find that using your config the camera on socket 3 would not get flashed, if I didn't specify the width and height.

Indeed, I have to set camera intrinsics with width and height, then the align node works !

tof-2024-08-27-144757.mp4

6MB

As you can see, it's not very well aligned for now, that's because I set the tof's K matrix to this :

        if tof:
            K = np.eye(3)
            K[0][0] = 640  # fx
            K[1][1] = 640  # fy
            K[0][2] = 320  # cx
            K[1][2] = 240  # cy
            tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket)
            ch.setCameraIntrinsics(tof_socket, K.tolist(), (640, 480))

How can I get the real intrinsic matrix of the ToF ? I guess it's flashed in it's own eeprom ?

apirrone

Also, we are using very wide FOV lenses, as you can see below

Is there an easy way to get the depth in full resolution and the relevant aligned rgb section cropped ? So that I have two 640x480 (depth and rgb) that are aligned ?

apirrone

jakaskerl Yes, the intrinsics are flashed in the module eeprom.

So no way to get them ? Do I need them to align the tof to RGB ?

I needed to set the camera intrinsics in the calibration handler for it to flash properly, but you said I had to set the size of the image, is there a way to set the image size without setting the intrinsics ?

How do you flash the calibration for the SR-PoE-ToF ?

jakaskerl You have to set it to CameraBoardSocket.AUTO instead of CameraBoardSocket.CAM_X. The enum value is -1.

In the end I set tof->left, left->tof and left->right, this satisfies this :

jakaskerl No two sockets can have the same toCameraSocket value

And seems to be enough

apirrone

jakaskerl Undistort the RGB image first. More on this here.

If I undistort the RGB image first, this is what I get :

No undistort :

Undistort:

apirrone

jakaskerl This script has never worked for me, that's why I made a custom calibration process.

I tried again still, in the tof_calib branch, with a fresh virtual environment and I have to iteratively comment out / edit / add things in the script to move on with the process. For example, for some reason, these parameters don't exist in this branch


                    tof_config.enablePhaseShuffleTemporalFilter = True
                    tof_config.enableFPPNCorrection = True
                    tof_config.enableOpticalCorrection = True
                    tof_config.enableWiggleCorrection = True
                    tof_config.enableTemperatureCorrection = True
                    tof_config.phaseUnwrappingLevel = 4
                    tof_config.phaseUnwrapErrorThreshold = 1000

while they exist here 2.28.0.0.dev0+76294530fd0bf91b09469490a1d14871c374630a.

Same for intensity, which means I'm stuck here luxonis/depthaiblob/28f2bd654923e24019f00248cf03925866b107af/calibrate.py#L608

So I recorded the images and computed the calibration using my usual process and I still have alignement issues.

This is where I flash the calibration : pollen-robotics/pollen-visionblob/01f1901a95f44e052b2953e4453247c6d7aa6b47/pollen_vision/pollen_vision/camera_wrappers/depthai/wrapper.py#L260

Now I get the intrinsics and extrinsics from my calibration process
It seems better than before :

But there is still a misalign when my arm is closer for example

Now, when I undistort the rgb first

jakaskerl Undistort the RGB image first. More on this here.

If doesn't change much because most of the distortion is on the edge of the FOV and the ToF's FOV is is much smaller than our fisheye lenses.

Also, the ToF can get pretty noisy sometimes (as you can see on top of the image below):

Do you have any advice ?

Thanks

jakaskerl

apirrone
Looks better, but the alignment is still off and the scale is wrong.
Can you post the calibration json as well?

Thanks,
Jaka

Hester

apirrone use surface mount resistor

apirrone

Hester I tried, but it's so small I couldn't solder it properly 🙂

Hester

apirrone There are different sizes

apirrone

jakaskerl

This is the json output of:

device_calibration_backup_file = Path("./CALIBRATION_BACKUP_" + now + ".json")
deviceCalib = self._device.readCalibration()
deviceCalib.eepromToJsonFile(device_calibration_backup_file)

{
    "batchName": "",
    "batchTime": 0,
    "boardConf": "",
    "boardCustom": "",
    "boardName": "",
    "boardOptions": 0,
    "boardRev": "",
    "cameraData": [
        [
            3,
            {
                "cameraType": 0,
                "distortionCoeff": [
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0
                ],
                "extrinsics": {
                    "rotationMatrix": [
                        [
                            1.0,
                            0.0,
                            0.0
                        ],
                        [
                            0.0,
                            1.0,
                            0.0
                        ],
                        [
                            0.0,
                            0.0,
                            1.0
                        ]
                    ],
                    "specTranslation": {
                        "x": 3.2509000301361084,
                        "y": 0.0,
                        "z": 0.0
                    },
                    "toCameraSocket": 2,
                    "translation": {
                        "x": 3.2509000301361084,
                        "y": 0.0,
                        "z": 0.0
                    }
                },
                "height": 480,
                "intrinsicMatrix": [
                    [
                        550.0,
                        0.0,
                        320.0
                    ],
                    [
                        0.0,
                        550.0,
                        240.0
                    ],
                    [
                        0.0,
                        0.0,
                        1.0
                    ]
                ],
                "lensPosition": 0,
                "specHfovDeg": 0.0,
                "width": 640
            }
        ],
        [
            2,
            {
                "cameraType": 1,
                "distortionCoeff": [
                    -0.017457952722907066,
                    0.04737565293908119,
                    -0.04832303524017334,
                    0.013992629945278168,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    0.0
                ],
                "extrinsics": {
                    "rotationMatrix": [
                        [
                            0.9999908804893494,
                            0.0038178323302417994,
                            -0.0019165652338415384
                        ],
                        [
                            -0.0038198132533580065,
                            0.9999921917915344,
                            -0.0010310197249054909
                        ],
                        [
                            0.0019126139814034104,
                            0.0010383311891928315,
                            0.999997615814209
                        ]
                    ],
                    "specTranslation": {
                        "x": 6.538896560668945,
                        "y": -0.017339760437607765,
                        "z": -0.021465661004185677
                    },
                    "toCameraSocket": 1,
                    "translation": {
                        "x": 6.538896560668945,
                        "y": -0.017339760437607765,
                        "z": -0.021465661004185677
                    }
                },
                "height": 1080,
                "intrinsicMatrix": [
                    [
                        534.3621215820313,
                        0.0,
                        707.8563232421875
                    ],
                    [
                        0.0,
                        532.467041015625,
                        557.4783325195313
                    ],
                    [
                        0.0,
                        0.0,
                        1.0
                    ]
                ],
                "lensPosition": 0,
                "specHfovDeg": 0.0,
                "width": 1440
            }
        ],
        [
            1,
            {
                "cameraType": 1,
                "distortionCoeff": [
                    0.0015351194888353348,
                    -0.0058898827992379665,
                    -0.0027657479513436556,
                    0.001210730872116983,
                    0.0,
                    0.0,
                    0.0,
                    0.0,
                    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": 1080,
                "intrinsicMatrix": [
                    [
                        534.84619140625,
                        0.0,
                        753.4248046875
                    ],
                    [
                        0.0,
                        532.37451171875,
                        537.838134765625
                    ],
                    [
                        0.0,
                        0.0,
                        1.0
                    ]
                ],
                "lensPosition": 0,
                "specHfovDeg": 0.0,
                "width": 1440
            }
        ]
    ],
    "deviceName": "",
    "hardwareConf": "",
    "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": "",
    "stereoEnableDistortionCorrection": false,
    "stereoRectificationData": {
        "leftCameraSocket": 2,
        "rectifiedRotationLeft": [
            [
                1.0,
                0.0,
                0.0
            ],
            [
                0.0,
                1.0,
                0.0
            ],
            [
                0.0,
                0.0,
                1.0
            ]
        ],
        "rectifiedRotationRight": [
            [
                0.9999908804893494,
                -0.0038198132533580065,
                0.0019126139814034104
            ],
            [
                0.0038178323302417994,
                0.9999921917915344,
                0.0010383311891928315
            ],
            [
                -0.0019165652338415384,
                -0.0010310197249054909,
                0.999997615814209
            ]
        ],
        "rightCameraSocket": 1
    },
    "stereoUseSpecTranslation": true,
    "version": 7,
    "verticalCameraSocket": -1
}

I'm still unsure on how to set the ToF's data in the calibration. I set this values manually, they seem to have an impact on the reprojection but I don't know how to get the real values.

            K[0][0] = 550  # fx
            K[1][1] = 550  # fy
            K[0][2] = 320  # cx
            K[1][2] = 240  # cy
            tof_socket = get_socket_from_name("tof", self.cam_config.name_to_socket)
            ch.setCameraIntrinsics(tof_socket, K.tolist(), (640, 480))

About undistorting the RGB before aligning, could explain more ? This is what my pipeline looks like right now

jakaskerl

apirrone
apirrone
These are intrinsics and distortion coefficients:

{
        "cameraType": 0,
        "distortionCoeff": [
          -9.466925621032715,
          30.354965209960938,
          0.0001632508501643315,
          -0.00029841947252862155,
          27.92099380493164,
          -9.42302131652832,
          29.685861587524414,
          30.283370971679688,
          0.0,
          0.0,
          0.0,
          0.0,
          -0.005233943462371826,
          -0.004971789661794901
        ],
        "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": 480,
        "intrinsicMatrix": [
          [
            471.8361511230469,
            0.0,
            322.25347900390625
          ],
          [
            0.0,
            471.7205810546875,
            246.209716796875
          ],
          [
            0.0,
            0.0,
            1.0
          ]
        ],
        "lensPosition": 0,
        "specHfovDeg": 71.86000061035156,
        "width": 640
      }

Try to use these values for intrinsics. Then check if the image improves.
For accurate extrinsics calibration, please use the script from here.

Thanks,
Jaka

apirrone

Thanks jakaskerl , I'll try these soon .

You said you would provide a script so that I could get these information from the ToF's eeprom, this would be very useful to us as we will need to configure many setups like this one in the future.

Thanks !

jakaskerl

apirrone
I can provide a script, but the team consensus during our sync was that the extrinsics are causing most of the issues.

Thanks,
Jaka

apirrone

Ok, I agree that the misalign is probably due to inaccurate extrinsics since I only set the specs translation.

So I should calibrate the ToF's extrinsics (and maybe I can do the intrinsics too at the same time in the end) using the amplitude output of the ToF node right ? https://docs.luxonis.com/software/depthai-components/nodes/tof/

As you do here : luxonis/depthaiblob/df80b87cf3f9e6f3255e246f2ce5e5cba0ce312a/calibrate.py#L453

I just have to acquire this image along with the left and right cameras, then I can perform the calibration. However, as this is an "amplitude" image, it's not grayscale between 0-255. How do you convert this data to a usable grayscale image on which you can detect aruco tags ?

Thanks !

jakaskerl

apirrone
Use branch luxonis/depthaitree/tof_calib, use a physical board (not a monitor). Run calibrate.py - everything should be working as expected, just make sure the board file is correct.

apirrone I just have to acquire this image along with the left and right cameras, then I can perform the calibration. However, as this is an "amplitude" image, it's not grayscale between 0-255. How do you convert this data to a usable grayscale image on which you can detect aruco tags ?

It's using amplitude, I think it's scaled to act as grayscale U8. The only thing that matters is that markers are detected.

Thanks,
Jaka

jakaskerl

apirrone
Maybe the custom calib process you are using produces errors, so I am going to try to fix the tof_calib process...

apirrone I tried again still, in the tof_calib branch, with a fresh virtual environment and I have to iteratively comment out / edit / add things in the script to move on with the process. For example, for some reason, these parameters don't exist in this branch

checkout this branch, but keep depthai==2.28, set numpy==1.26.4 and opencv-contrib-python==4.6.0.66.

Then run the calibration and try again. You should not have to remove anything.

Thanks,
Jaka

apirrone

@jakaskerl I have a new problem

I was messing around with calibration, and suddenly (well I guess It happened after I flashed a new test calibration), the raw intensity output of the ToF is all distorted :

This obviously looks like bad distortion coefficients, but now I can't get a raw intensity image without this distortion ... Even with this minimal code :

import depthai as dai
import cv2

pipeline = dai.Pipeline()

cam_tof = pipeline.create(dai.node.Camera)
cam_tof.setFps(30)

cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_D)

tof = pipeline.create(dai.node.ToF)
cam_tof.raw.link(tof.input)

output = pipeline.create(dai.node.XLinkOut)
output.setStreamName("intensity")

tof.intensity.link(output.input)

device = dai.Device(pipeline)

q = device.getOutputQueue(name="intensity", maxSize=8, blocking=False)

while True:
    data = q.get()
    cv2.imshow("intensity", data.getCvFrame())
    cv2.waitKey(1)

I was also messing around with depthai versions, I built the latest version of develop from source :

2.28.0.0.dev0+d3b92a92782894b33015d97d435451ff35532d06

It's more recent than the version you told me to use I guess

Luxonis-Alex depthai version currently

So I went back to this version 76294530fd0bf91b09469490a1d14871c374630a and same thing.

So now I can't get raw data for calibration. Any idea what happened ?

apirrone

Ok I saw this in the latest changelog

Adding this solved my problem

« Previous Page Next Page »