ToF bad accuracy

LLuxonis-Alex · Aug 27, 2024

@apirrone We'll follow up on this question about alignment.

For the issue with IMX296 RPi GS Cam not working together with the ToF, would you be able to try a rework on the SL6996 R1M1E1 cable adapter boards, it would be replacing this 10K resistor (the populated one) with a 1K resistor. Or if easier, just soldering a 1K resistor in parallel with the existing 10K one (on top of it), final value isn't that important:

Design files of that adapter: luxonis/depthai-hardwaretree/master/SL6996_OAK-FFC_15pin-RPi

apirrone · Aug 27, 2024

Luxonis-Alex I'll try that right now

apirrone · Aug 27, 2024

It was hard, but I managed to solder a 1k resistor this way

And ... it works ! I no longer have the freeze ! I only tested with one camera for now, I'll try soldering another resistor to the other board to see how it behaves with the two cameras

Thanks @Luxonis-Alex ! Can you explain what was going on ?

apirrone · Aug 27, 2024

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],
)

apirrone · Aug 27, 2024

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 · Aug 28, 2024

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 ?

jakaskerl · Aug 28, 2024

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 · Aug 29, 2024

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 · Aug 29, 2024

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:

HHester · Aug 29, 2024

apirrone use surface mount resistor

apirrone · Aug 30, 2024

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

HHester · Aug 31, 2024

apirrone There are different sizes

jakaskerl · Sep 1, 2024

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

Thanks,
Jaka

apirrone · Sep 3, 2024

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 · Sep 11, 2024

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 · Sep 11, 2024

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 · Sep 12, 2024

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 · Sep 19, 2024

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 · Sep 20, 2024

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

apirrone · Sep 24, 2024

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