Handle distortion in the ipm

ipm_library/ipm_library/utils.py

@@ -15,6 +15,7 @@
 from typing import Optional, Tuple
 
 from builtin_interfaces.msg import Time
+import cv2
 from geometry_msgs.msg import Transform
 import numpy as np
 from rclpy.duration import Duration
@@ -102,30 +103,38 @@ def get_field_intersection_for_pixels(
         points: np.ndarray,
         plane_normal: np.ndarray,
         plane_base_point: np.ndarray,
-        scale: float = 1.0) -> np.ndarray:
+        scale: float = 1.0,
+        use_distortion: bool = False) -> np.ndarray:
     """
     Map a NumPy array of points in image space on the given plane.
 
     :param points: A nx2 array with n being the number of points
     :param plane_normal: The normal vector of the mapping plane
     :param plane_base_point: The base point of the mapping plane
     :param scale: A scaling factor used if e.g. a mask with a lower resolution is transformed
+    :param use_distortion: A flag to indicate if distortion should be accounted for.
+        Do not use this if you are working with pixel coordinates from a rectified image.
     :returns: A NumPy array containing the mapped points
         in 3d relative to the camera optical frame
     """
-    camera_projection_matrix = camera_info.k
-
-    # Calculate binning and scale
+    # Apply binning and scale
     binning_x = max(camera_info.binning_x, 1) / scale
     binning_y = max(camera_info.binning_y, 1) / scale
-
-    # Create rays
-    ray_directions = np.zeros((points.shape[0], 3))
-    ray_directions[:, 0] = ((points[:, 0] - (camera_projection_matrix[2] / binning_x)) /
-                            (camera_projection_matrix[0] / binning_x))
-    ray_directions[:, 1] = ((points[:, 1] - (camera_projection_matrix[5] / binning_y)) /
-                            (camera_projection_matrix[4] / binning_y))
-    ray_directions[:, 2] = 1
+    points = points * np.array([binning_x, binning_y])
+
+    # Create identity distortion coefficients if no distortion is used
+    if use_distortion:
+        distortion_coefficients = np.array(camera_info.d)
+    else:
+        distortion_coefficients = np.zeros(5)
+
+    # Get the ray directions relative to the camera optical frame for each of the points
+    ray_directions = np.ones((points.shape[0], 3))
+    ray_directions[:, :2] = cv2.undistortPoints(
+        points.reshape(1, -1, 2).astype(np.float32),
+        np.array(camera_info.k).reshape(3, 3),
+        distortion_coefficients,
+        R=np.array(camera_info.r).reshape(3, 3)).reshape(-1, 2)
 
     # Calculate ray -> plane intersections
     intersections = line_plane_intersections(

ipm_library/package.xml

@@ -11,11 +11,12 @@
   <depend>geometry_msgs</depend>
   <depend>ipm_interfaces</depend>
   <depend>python3-numpy</depend>
+  <depend>python3-opencv</depend>
   <depend>sensor_msgs</depend>
   <depend>shape_msgs</depend>
   <depend>std_msgs</depend>
-  <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>tf2</depend>
   <depend>vision_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>

ipm_library/test/test_ipm.py

@@ -36,7 +36,10 @@
     height=1536,
     binning_x=4,
     binning_y=4,
-    k=[1338.64532, 0., 1026.12387, 0., 1337.89746, 748.42213, 0., 0., 1.])
+    k=[1338.64532, 0., 1026.12387, 0., 1337.89746, 748.42213, 0., 0., 1.],
+    d=np.zeros(5),
+    r=np.eye(3).flatten(),
+    )
 
 
 def test_ipm_camera_info():
@@ -127,7 +130,7 @@ def test_ipm_map_points_no_transform():
     # Projection doesn't consider the binning, so we need to correct for that
     point_projected_2d[0] = point_projected_2d[0] / camera_info.binning_x
     point_projected_2d[1] = point_projected_2d[1] / camera_info.binning_y
-    assert np.allclose(points, np.transpose(point_projected_2d), rtol=0.0001), \
+    assert np.allclose(points, np.transpose(point_projected_2d), rtol=0.001, atol=0.001), \
         'Mapped point differs too much'