Handle distortion in the ipm

ipm_image_node/ipm_image_node/ipm.py

@@ -35,17 +35,18 @@ class IPMImageNode(Node):
 
     def __init__(self) -> None:
         super().__init__('ipm_image_node')
+        # Declare params
+        self.declare_parameter('output_frame', 'base_footprint')
+        self.declare_parameter('type', 'mask')
+        self.declare_parameter('scale', 1.0)
+        self.declare_parameter('use_distortion', False)
+
         # We need to create a tf buffer
         self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30.0))
         self.tf_listener = tf2.TransformListener(self.tf_buffer, self)
 
         # Create an IPM instance
-        self.ipm = IPM(self.tf_buffer)
-
-        # Declare params
-        self.declare_parameter('output_frame', 'base_footprint')
-        self.declare_parameter('type', 'mask')
-        self.declare_parameter('scale', 1.0)
+        self.ipm = IPM(self.tf_buffer, distortion=self.get_parameter('use_distortion').value)
 
         # Subscribe to camera info
         self.create_subscription(CameraInfo, 'camera_info', self.ipm.set_camera_info, 1)

ipm_library/ipm_library/ipm.py

@@ -35,7 +35,8 @@ class IPM:
     def __init__(
             self,
             tf_buffer: tf2_ros.Buffer,
-            camera_info: Optional[CameraInfo] = None) -> None:
+            camera_info: Optional[CameraInfo] = None,
+            distortion: bool = False) -> None:
         """
         Create a new inverse perspective mapper instance.
 
@@ -44,10 +45,13 @@ def __init__(
             camera intrinsics, camera frame, ...
             The camera info can be updated later on using the setter or
             provided directly if it is unlikly to change
+        :param distortion: Weather to use the distortion coefficients from the camera info.
+            Don't use this if you are using a rectified image.
         """
         # TF needs a listener that is init in the node context, so we need a reference
         self._tf_buffer = tf_buffer
         self.set_camera_info(camera_info)
+        self._distortion = distortion
 
     def set_camera_info(self, camera_info: CameraInfo) -> None:
         """
@@ -178,7 +182,8 @@ def map_points(
             self._camera_info,
             points,
             plane_normal,
-            plane_base_point)
+            plane_base_point,
+            use_distortion=self._distortion)
 
         # Transform output point if output frame if needed
         if output_frame_id not in [None, self._camera_info.header.frame_id]:

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))
+    if points.shape[0] > 0:
+        ray_directions[:, :2] = cv2.undistortPoints(
+            points.reshape(1, -1, 2).astype(np.float32),
+            np.array(camera_info.k).reshape(3, 3),
+            distortion_coefficients).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,9 @@
     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),
+    )
 
 
 def test_ipm_camera_info():
@@ -127,7 +129,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'
 
 

ipm_service/ipm_service/ipm.py

@@ -28,11 +28,13 @@ class IPMService(Node):
 
     def __init__(self) -> None:
         super().__init__('ipm_service')
+        # Declare params
+        self.declare_parameter('use_distortion', False)
         # TF handling
         self.tf_buffer = tf2.Buffer(Duration(seconds=5))
         self.tf_listener = tf2.TransformListener(self.tf_buffer, self)
         # Create ipm library instance
-        self.ipm = IPM(self.tf_buffer)
+        self.ipm = IPM(self.tf_buffer, distortion=self.get_parameter('use_distortion').value)
         # Create subs
         self.camera_info_sub = self.create_subscription(
             CameraInfo, 'camera_info', self.ipm.set_camera_info, 1)