Simplify camera transformations with the new Frame3d-Feature from RoSys

[rodja]

Since the next RoSys release will fix zauberzeug/rosys#130, this PR utilizes the new Frame3d to get rid of hacks throughout the codebase. While at it, I fixed some typing issues, made some cleanups and extracted/renamed variables to improve readability and consistency.

NOTE: this branch does only work with the RoSys main line at the moment and should only be merged after RoSys 0.14.0 has been released.

ToDos

• fix tests
• wait for RoSys 0.14.0 and update requirements.txt
• test camera card's tool augmentation and relevant weeds augmentation on real robot
• test plant locator is proper functioning on real robot
• review code
• make sure we can load the old camera calibration configs

[rodja]

I fixed the test_follow_crops_outlier test by increasing the error margin from 1° to 2°. The introduction of the Frame3d seems to skew the orientation a bit different than before. To verify that 2° of error is still ok, I added multiple outliers to the follow crops simulation which looks quite good to me.

[rodja]

@pascalzauberzeug or @angelom93: you can review and test the code already (with rosys main branch). That way we could merge as soon as the 0.14.0 version of RoSys get's released.

[pascalzauberzeug]

@rodja I already checked the code, but don't have time to test it on a robot until tuesday
@angelom93 you can test it if you find the time, otherwise I will do it on tuesday

[pascalzauberzeug]

I have updated rosys to v0.14.0 and merged main into this branch.
We have to push this forward tomorrow, because #149 depends on v0.14.0 and needs this PR for it to work

[angelom93]

@rodja @pascalzauberzeug I tried to get this PR working on a real robot. After fixing some issues with the calibration dialog and camera card, I encountered a bigger problem. In line 210 of camera_card.py, we try to get the image position of the world coordinates, including the odometer position of the robot:

tool_3d = self.odometer.prediction.point_3d() + \
            rosys.geometry.Point3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.position, z=0)
tool_2d = self.camera.calibration.project_to_image(tool_3d) / self.shrink_factor

I think we need to pass the robot’s pose as frame parameter. As the calibration itself doesn´t know the current frame of the camera. The same issue occurs in the plant_locator in the other direction.

image_point = rosys.geometry.Point(x=d.cx, y=d.cy)
world_point_3d = camera.calibration.project_from_image(image_point)

Additionally, there was a problem with the old calibration on the robot, as it seems to be incompatible with the new code. Upgrading would require us to either recalibrate all robots or create a calibration import method.

[rodja]

@angelom93 Wow. Thanks for uncovering these issues. Let's dive in:

I think we need to pass the robot’s pose as frame parameter. As the calibration itself doesn´t know the current frame of the camera.

The camera pose should have already a associated frame which automatically should be used when doing the projection. At least that was true when testing in simulation. Maybe the frame is not set correctly on a real robot?
With your changes in ca81e46795f55 the tool display in the camera does not work anymore in the simulation.

Additionally, there was a problem with the old calibration on the robot, as it seems to be incompatible with the new code. Upgrading would require us to either recalibrate all robots or create a calibration import method.

That is problematic. How much effort would it be to write such a calibration import step? Maybe that is better than recalibrating every robot?

[pascalzauberzeug]

@rodja I don't have a new calibration file on hand, but I think it shouldn't be too hard to write a converter. When looking at calibration in 0.14.0, extrinsics is a Pose3D and before it was it's own class.
I think this should be the main difference

[pascalzauberzeug]

Here is an example of a broken calibration from F13/RB36

cat field_friend.vision.calibratable_usb_camera_provider.json

{
    "cameras": {
        "32e4-9230-platform-3610000.xhci-usb-0:2.4:1.0": {
            "id": "32e4-9230-platform-3610000.xhci-usb-0:2.4:1.0",
            "name": "32e4-9230-platform-3610000.xhci-usb-0:2.4:1.0",
            "connect_after_init": true,
            "streaming": false,
            "focal_length": 1830,
            "calibration": {
                "intrinsics": {
                    "model": "pinhole",
                    "matrix": [
                        [
                            704.3472414299275,
                            0.0,
                            515.2572427719205
                        ],
                        [
                            0.0,
                            709.5225125575603,
                            329.3347043150279
                        ],
                        [
                            0.0,
                            0.0,
                            1.0
                        ]
                    ],
                    "distortion": [
                        -0.42323083237038744,
                        0.24994860543758948,
                        -0.0016619969416450243,
                        -7.288107826439712e-05,
                        -0.07725206082664601
                    ],
                    "xi": 0.0,
                    "rotation": {
                        "R": [
                            [
                                1,
                                0,
                                0
                            ],
                            [
                                0,
                                1,
                                0
                            ],
                            [
                                0,
                                0,
                                1
                            ]
                        ]
                    },
                    "size": {
                        "width": 1020,
                        "height": 700
                    }
                },
                "extrinsics": {
                    "rotation": {
                        "R": [
                            [
                                0.0012156952531017229,
                                0.8037678718850602,
                                -0.5949417872451164
                            ],
                            [
                                0.9999974845579332,
                                -0.0020985613297158645,
                                -0.0007917816314380813
                            ],
                            [
                                -0.001884930465043511,
                                -0.5949393281383465,
                                -0.8037684012645899
                            ]
                        ]
                    },
                    "translation": [
                        0.0,
                        0.0,
                        1.0
                    ]
                }
            },
            "auto_exposure": true,
            "exposure": false,
            "width": 1280,
            "height": 720,
            "fps": 9
        },
        "32e4-9230-platform-3610000.xhci-usb-0:2.3:1.0": {
            "id": "32e4-9230-platform-3610000.xhci-usb-0:2.3:1.0",
            "name": "32e4-9230-platform-3610000.xhci-usb-0:2.3:1.0",
            "connect_after_init": true,
            "streaming": true,
            "focal_length": 1830,
            "calibration": {
                "intrinsics": {
                    "model": "pinhole",
                    "matrix": [
                        [
                            704.3472414299275,
                            0.0,
                            515.2572427719205
                        ],
                        [
                            0.0,
                            709.5225125575603,
                            329.3347043150279
                        ],
                        [
                            0.0,
                            0.0,
                            1.0
                        ]
                    ],
                    "distortion": [
                        -0.42323083237038744,
                        0.24994860543758948,
                        -0.0016619969416450243,
                        -7.288107826439712e-05,
                        -0.07725206082664601
                    ],
                    "xi": 0.0,
                    "rotation": {
                        "R": [
                            [
                                1,
                                0,
                                0
                            ],
                            [
                                0,
                                1,
                                0
                            ],
                            [
                                0,
                                0,
                                1
                            ]
                        ]
                    },
                    "size": {
                        "width": 1020,
                        "height": 700
                    }
                },
                "extrinsics": {
                    "rotation": {
                        "R": [
                            [
                                0.0012156952531017229,
                                0.8037678718850602,
                                -0.5949417872451164
                            ],
                            [
                                0.9999974845579332,
                                -0.0020985613297158645,
                                -0.0007917816314380813
                            ],
                            [
                                -0.001884930465043511,
                                -0.5949393281383465,
                                -0.8037684012645899
                            ]
                        ]
                    },
                    "translation": [
                        0.0,
                        0.0,
                        1.0
                    ]
                }
            },
            "auto_exposure": true,
            "exposure": false,
            "width": 1280,
            "height": 720,
            "fps": 9
        }
    }
}

[pascalzauberzeug]

@rodja The camera and projection movement are now fixed for the 3dScene with zauberzeug/rosys#183.
It still needs to be tested on a real robot and the implement projection of the camera card is still broken, but @angelom93 and I think that the camera card should be adressed in another PR to get this one merged

[rodja]

Sounds good to me!