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

@@ -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.
 

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)