From ffbdc376183e135d44bef28ecfa3783cc4a503fe Mon Sep 17 00:00:00 2001 From: Niklas Neugebauer Date: Thu, 6 Feb 2025 10:54:19 +0100 Subject: [PATCH] feat: implement redistortion for the pinhole model tests: add tests for redistortion style: simplify normalization for fisheye model style: explicitly set unused crop to False for pinhole undistortion --- rosys/vision/calibration.py | 22 ++++++++++++++----- tests/test_camera_calibration.py | 37 ++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 5 deletions(-) diff --git a/rosys/vision/calibration.py b/rosys/vision/calibration.py index fab62e81..ca756fd4 100644 --- a/rosys/vision/calibration.py +++ b/rosys/vision/calibration.py @@ -285,7 +285,7 @@ def undistort_points(self, image_points: np.ndarray, crop: bool = False) -> np.n K = np.array(self.intrinsics.matrix, dtype=np.float32).reshape((3, 3)) D = np.array(self.intrinsics.distortion) if self.intrinsics.model == CameraModel.PINHOLE: - new_K = self.get_undistorted_camera_matrix(crop=crop) + new_K = self.get_undistorted_camera_matrix(crop=False) return cv2.undistortPoints(image_points, K, D, P=new_K, R=np.eye(3)) elif self.intrinsics.model == CameraModel.FISHEYE: new_K = self.get_undistorted_camera_matrix(crop=crop) @@ -300,6 +300,7 @@ def undistort_points(self, image_points: np.ndarray, crop: bool = False) -> np.n def distort_points(self, image_points: np.ndarray, crop: bool = False) -> np.ndarray: """Generalized wrapper for distorting image points. + Note: For pinhole models the redistortion can be off by more than 1px for large distortions. :param image_points: The image points to distort. :param crop: Whether cropping is applied to the image during distortion. @@ -310,18 +311,29 @@ def distort_points(self, image_points: np.ndarray, crop: bool = False) -> np.nda D = np.array(self.intrinsics.distortion) if self.intrinsics.model == CameraModel.PINHOLE: - new_K = self.get_undistorted_camera_matrix(crop=crop) - return cv2.undistortPoints(image_points, K, D, P=new_K, R=np.eye(3)) + return self._distort_points_pinhole(image_points) elif self.intrinsics.model == CameraModel.FISHEYE: new_K = self.get_undistorted_camera_matrix(crop=crop) - normalized_points = np.linalg.inv(new_K) @ cv2.convertPointsToHomogeneous(image_points).reshape(-1, 3).T - normalized_points = cv2.convertPointsFromHomogeneous(normalized_points.T).reshape(-1, 1, 2) + normalized_points = cv2.undistortPoints(image_points, new_K, None) return cv2.fisheye.distortPoints(normalized_points, K, D) elif self.intrinsics.model == CameraModel.OMNIDIRECTIONAL: raise NotImplementedError('Re-distortion for omnidirectional cameras is not supported') else: raise ValueError(f'Unknown camera model "{self.intrinsics.model}"') + def _distort_points_pinhole(self, image_points: np.ndarray) -> np.ndarray: + K = np.array(self.intrinsics.matrix, dtype=np.float32) + D = np.array(self.intrinsics.distortion, dtype=np.float32) + new_K = self.get_undistorted_camera_matrix(crop=False) + + # Note: this is a slight hack of available functions. + # We first normalize the points, then project them + # In the end this applies only the distortion during projectPoints + normalized_points = cv2.undistortPoints(image_points, new_K, None) + points_3d = cv2.convertPointsToHomogeneous(normalized_points) + distorted_points, _ = cv2.projectPoints(points_3d, np.zeros(3), np.zeros(3), K, D) + return distorted_points + def get_undistorted_camera_matrix(self, crop: bool = False) -> np.ndarray: """Compute the camera matrix for the undistorted image. diff --git a/tests/test_camera_calibration.py b/tests/test_camera_calibration.py index 63200bef..94d775ac 100644 --- a/tests/test_camera_calibration.py +++ b/tests/test_camera_calibration.py @@ -1,6 +1,7 @@ import copy import numpy as np +import pytest from rosys.geometry import Point3d, Pose3d from rosys.geometry.object3d import frame_registry @@ -320,3 +321,39 @@ def test_omnidirectional_project_from_behind(): cam.calibration.intrinsics.model = CameraModel.OMNIDIRECTIONAL assert cam.calibration.project_to_image(Point3d(x=0, y=1, z=1)) is not None assert cam.calibration.project_to_image(Point3d(x=0, y=-1, z=1)) is not None + + +@pytest.mark.parametrize("distortion", [ + [0.11, -0.12, 0.13, -0.14], + [0.11, -0.12, 0.13, -0.14, 0.15], + [20.13, 5.89, 0.0001, -0.00025, 0.0706, 21.58, 13.108, 0.8059], + [20.13, 5.89, 0.0001, -0.00025, 0.0706, 21.58, 13.108, 0.8059, 0.0001, -0.0003, 0.0678, -0.0012], + [20.13, 5.89, 0.0001, -0.00025, 0.0706, 21.58, 13.108, 0.8059, 0.0001, -0.0003, 0.0678, -0.0012, 0.0004, -0.0005], +]) +def test_distort_points_pinhole(distortion: list[float]): + cam = CalibratableCamera(id='1') + cam.set_perfect_calibration(z=4) + assert cam.calibration is not None + + cam.calibration.intrinsics.distortion = distortion + cam.calibration.intrinsics.model = CameraModel.PINHOLE + + points = np.array([[100, 100], [200, 200], [300, 300], [400, 400]], dtype=np.float32).reshape((-1, 1, 2)) + undistorted_points = cam.calibration.undistort_points(points) + redistorted_points = cam.calibration.distort_points(undistorted_points) + assert np.allclose(points, redistorted_points, atol=0.4) + + +@pytest.mark.parametrize("crop", [True, False]) +def test_distort_points_fisheye(crop: bool): + cam = CalibratableCamera(id='1') + cam.set_perfect_calibration(z=4, roll=np.deg2rad(180 + 10)) + assert cam.calibration is not None + + cam.calibration.intrinsics.distortion = [-0.014, -0.0023, 0.001, 0.001] + cam.calibration.intrinsics.model = CameraModel.FISHEYE + + points = np.array([[100, 100], [200, 200], [300, 300], [400, 400]], dtype=np.float32).reshape((-1, 1, 2)) + undistorted_points = cam.calibration.undistort_points(points, crop=crop) + redistorted_points = cam.calibration.distort_points(undistorted_points, crop=crop) + assert np.allclose(points, redistorted_points, atol=1e-6)