diff --git a/ipm_image_node/ipm_image_node/ipm.py b/ipm_image_node/ipm_image_node/ipm.py
index 5ad4569..85b55c8 100644
--- a/ipm_image_node/ipm_image_node/ipm.py
+++ b/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)
diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py
index e3dfb29..34c8586 100644
--- a/ipm_library/ipm_library/ipm.py
+++ b/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]:
diff --git a/ipm_library/ipm_library/utils.py b/ipm_library/ipm_library/utils.py
index bd92fb0..d1ff6ea 100644
--- a/ipm_library/ipm_library/utils.py
+++ b/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,7 +103,8 @@ 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.
@@ -110,22 +112,29 @@ def get_field_intersection_for_pixels(
: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(
diff --git a/ipm_library/package.xml b/ipm_library/package.xml
index d7790ec..f266013 100644
--- a/ipm_library/package.xml
+++ b/ipm_library/package.xml
@@ -11,11 +11,12 @@
geometry_msgs
ipm_interfaces
python3-numpy
+ python3-opencv
sensor_msgs
shape_msgs
std_msgs
- tf2
tf2_geometry_msgs
+ tf2
vision_msgs
ament_copyright
diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py
index cef2d36..e0ff930 100644
--- a/ipm_library/test/test_ipm.py
+++ b/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'
diff --git a/ipm_service/ipm_service/ipm.py b/ipm_service/ipm_service/ipm.py
index df0e792..542640a 100644
--- a/ipm_service/ipm_service/ipm.py
+++ b/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)