Undistorting World and Depth Camera Images
This post details instructions and examples for undistorting images and points captured by the world and depth cameras in the Magic Leap 2 (ML2) device. It includes the implementation of custom distortion models for the world camera and the use of standard OpenCV models for the depth camera.
Table of Contents
World Camera
The world camera utilizes an equidistant camera model, which is somewhat akin to OpenCV's fisheye model but with important distinctions that necessitate a specific approach for distortion correction.
Implementation
Below is the implementation of the distortion model specifically tailored for the ML2 world camera. It comprises methods for both distorting and undistorting points in images, taking into account radial and tangential distortion parameters.
import numpy as np
class WcamDistortionModel:
"""Magic Leap two stage distortion model
Description:
# Radial Distortion:
r = sqrt(x^2 + y^2)
θ = atan( r )
θ_rad = θ * (1 + k1 * θ^2 + k2 * θ^4 + k3 * θ^6 + k4 * θ^8)
x_rad = x * ( θ_rad / r )
y_rad = y * ( θ_rad / r )
# Tangential Distortion (applied after radial)
r_rad_sq = x_rad^2 + y_rad^2
x_rad_tan = x_rad + 2 * p1 * x_rad * y_rad + p2 * (r_rad_sq + 2 * x_rad^2)
y_rad_tan = y_rad + p1 * (r_rad_sq + 2 * y_rad^2) + 2 * p2 * x_rad * y_rad
Args:
K (np.ndarray): Camera matrix (3x3)
D (np.ndarray): Distortion coeffictions [k1, k2, p1, p2, k3, k4]
where k are radial coeffs and p are tangential coeffs
Raises:
ValueError: Wrong number of distortion parameters
Methods:
undistort_points
distort_points
"""
def __init__(self, K: np.ndarray, D: np.ndarray, width: int, height: int):
if len(D) != 6:
raise ValueError(f"Wrong length of distortion coefficients for custom ML equidistant distortion {len(D)}")
self.D, self.K = D, K
self.width, self.height = width, height
def __get_intrinsics__(self):
k1, k2 = self.D[0], self.D[1]
k3, k4 = self.D[4], self.D[5]
p1, p2 = self.D[2], self.D[3]
fx, fy = self.K[0, 0], self.K[1, 1]
cx, cy = self.K[0, 2], self.K[1, 2]
return k1, k2, k3, k4, p1, p2, fx, fy, cx, cy
def undistort_points(self, uv_d: np.ndarray, num_iters: int = 3) -> np.ndarray:
"""Return undistorted points for custom two-step equidistant camera model
Args:
uv_d (np.ndarray): 2D points in distorted image pixel space size Nx2
num_iters (int): Number of iterations for undistortion
Returns:
np.ndarray: Undistorted points in image pixel space of size Nx2
"""
uv_d = uv_d.reshape((-1, 2))
num_points = uv_d.shape[0]
k1, k2, k3, k4, p1, p2, fx, fy, cx, cy = self.__get_intrinsics__()
# convert from pixel coordinates to metric
xy_d = (uv_d - np.array([[cx, cy]])) / np.array([fx, fy])
# tangential component
xy = xy_d.copy()
x, y = xy[..., 0], xy[..., 1]
for k in range(num_iters):
x2 = x * x
y2 = y * y
x_y = x * y
r2 = x2 + y2
x_d_pred = x + 2.0 * p1 * x_y + p2 * (r2 + 2.0 * x2)
y_d_pred = y + 2.0 * p2 * x_y + p1 * (r2 + 2.0 * y2)
xy_d_pred = np.column_stack((x_d_pred, y_d_pred))
J = np.zeros((num_points, 2, 2))
J[..., 0, 0] = 1 + 2 * p1 * y + 6 * p2 * x
J[..., 0, 1] = 2 * p1 * x + 2 * p2 * y
J[..., 1, 0] = 2 * p1 * x + 2 * p2 * y
J[..., 1, 1] = 1 + 6 * p2 * y + 2 * p2 * x
J_inv = np.linalg.inv(J)
delta_xy = np.matmul(J_inv, (xy_d - xy_d_pred)[..., None])[..., 0]
xy += delta_xy
# radial component
xy_d = xy
theta_d = np.linalg.norm(xy_d, axis=-1, keepdims=True)
theta_d = np.where(theta_d > np.pi / 2, np.pi / 2, theta_d)
theta = theta_d.copy()
for k in range(num_iters):
theta2 = theta * theta
theta4 = theta2 * theta2
theta6 = theta2 * theta4
theta8 = theta2 * theta6
delta_theta = (
theta_d
- theta * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8)
) / (
1
+ 3 * k1 * theta2
+ 5 * k2 * theta4
+ 7 * k3 * theta6
+ 9 * k4 * theta8
)
theta = theta + delta_theta
r = np.tan(theta)
radial_inv = r / theta_d
xy = xy_d * radial_inv
# convert back to pixel coordinates
uv_ud = xy * np.array([fx, fy]) + np.array([cx, cy])
return uv_ud
def distort_points(self, uv_ud: np.ndarray) -> np.ndarray:
"""Return distorted points for two-step equidistant camera model
Args:
uv_ud (np.ndarray): 2D points in image pixel space size Nx2
Returns:
np.ndarray: Distorted points in image pixel space of size Nx2
"""
k1, k2, k3, k4, p1, p2, fx, fy, cx, cy = self.__get_intrinsics__()
# convert from pixel coordinates to metric
xy_ud = (uv_ud - np.array([[cx, cy]])) / np.array([fx, fy])
Ri = np.sqrt(np.sum(xy_ud**2, axis=1))
theta = np.arctan(Ri)
theta2 = theta * theta
theta4 = theta2 * theta2
theta6 = theta2 * theta4
theta8 = theta2 * theta6
rd = theta * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8)
rd = rd / Ri
# Polar to cartesian
x = xy_ud[:, 0] * rd
y = xy_ud[:, 1] * rd
if p2 != 0.0:
x2 = x * x
y2 = y * y
x_y = x * y
r2 = x2 + y2
x = x + 2.0 * p1 * x_y + p2 * (r2 + 2.0 * x2)
y = y + 2.0 * p2 * x_y + p1 * (r2 + 2.0 * y2)
# Convert to pixel coordinates
x = fx * x + cx
y = fy * y + cy
return np.vstack((x, y)).T
Undistortion of WCAM Images
To quickly undistort full images captured by the world camera, it's recommended to generate mapping arrays for use with OpenCV's remap function. This can be done using the distort_points method of our distortion model as illustrated below.
import cv2
import json
from matplotlib import pyplot as plt
# load wcam intrinsics parameters
with open("test_data/wcam_intrinsics.json") as f:
wcam_intr = json.load(f)
wcam = WcamDistortionModel(np.array(wcam_intr["K"]), wcam_intr["D"], wcam_intr["width"], wcam_intr["height"])
[X, Y] = np.meshgrid(np.arange(wcam.width), np.arange(wcam.height), indexing="xy")
ij = np.vstack([X.ravel(), Y.ravel()]).transpose()
pts = wcam.distort_points(ij)
map1 = pts[:, 0].reshape(wcam.width, wcam.height).astype(np.float32)
map2 = pts[:, 1].reshape(wcam.width, wcam.height).astype(np.float32)
# load example wcam image
img = cv2.imread("test_data/wcam_img.png")
img_ud = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR)
fig, ax = plt.subplots(1,2, figsize=(12,5))
ax[0].imshow(img)
ax[0].set_title("Original WCAM image")
ax[1].imshow(img_ud)
ax[1].set_title("Undistorted WCAM image")
plt.show()
Distorting and Undistorting of Individual Pixels
For situations requiring the manipulation of individual pixels, the undistort_points method can correct the distortion on a per-pixel basis as shown in the following example.
# pixel in distorted image:
pts = np.array([[200,300], [755,795], [825, 130]])
for pt in pts:
cv2.circle(img, pt, radius=10, color=(0,255,0), thickness=3)
# undistort and plot in undistorted image:
pts_ud = wcam.undistort_points(pts, 5)
for pt in pts_ud:
cv2.circle(img_ud, pt.astype(int), radius=10, color=(0,255,0), thickness=3)
fig, ax = plt.subplots(1,2, figsize=(12,5))
ax[0].imshow(img)
ax[0].set_title("Original WCAM image + points")
ax[1].imshow(img_ud)
ax[1].set_title("Undistorted WCAM image + undistorted points")
plt.show()
Depth Camera
The depth camera adheres to a standard pinhole camera model, which includes three radial distortion parameters and two tangential distortion parameters. OpenCV's functionality can be leveraged to undistort images captured by the depth camera.
Undistort Image and Points using OpenCV Pinhole Model
To correct distortion in depth camera images, you can utilize OpenCV's undistortion functions. The process may involve generating an undistortion map for efficient real-time applications. Below is an example of undistorting a depth image and correcting the positions of specific points within it.
# load depth camera intrinsics
with open("test_data/dcam_intrinsics.json") as f:
dcam_intr = json.load(f)
size = (dcam_intr["width"], dcam_intr["height"])
cameraMatrix = np.array(dcam_intr["K"])
distCoeffs = np.array(dcam_intr["D"])
# load example depth image and undistort
img = cv2.imread("test_data/dcam_img.png")
K_ud, roi_ud = cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, size, 0, size)
img_ud = cv2.undistort(img, cameraMatrix, distCoeffs, None, K_ud)
# points in distorted image:
pts = np.array([[50,106], [358,164], [432, 420]])
for pt in pts:
cv2.circle(img, pt, radius=10, color=(0,255,0), thickness=2)
# get undistorted points
pts_ud = cv2.undistortPoints(pts.astype(np.float32), cameraMatrix, distCoeffs)
# convert back to pixel coordinates
fx, fy = K_ud[0,0], K_ud[1,1]
cx, cy = K_ud[0,2], K_ud[1,2]
pts_ud = pts_ud * np.array([fx, fy]) + np.array([cx, cy])
for pt in pts_ud:
cv2.circle(img_ud, pt.reshape((2,)).astype(int), radius=10, color=(0,255,0), thickness=2)
fig, ax = plt.subplots(1,2, figsize=(12,5))
ax[0].imshow(img)
ax[0].set_title("Original depth image + points")
ax[1].imshow(img_ud)
ax[1].set_title("Undistorted depth image + undistorted points")
plt.show()
This guide provides a comprehensive overview of handling image and point undistortion for both the world and depth cameras on the ML2 device, ensuring clear and accurate visual data for your applications.
Undistort image with manually created undistortion map
Using a Python implementation for the radial+tangential distortion pinhole model instead of OpenCV.
# depth camera manually created UD map
with open("test_data/dcam_intrinsics.json") as f:
dcam_intr = json.load(f)
width, height = dcam_intr["width"], dcam_intr["height"]
size = (width, height)
K = np.array(dcam_intr["K"])
k1, k2, p1, p2, k3 = dcam_intr["D"]
fx, fy, cx, cy = K[0,0], K[1,1], K[0,2], K[1,2]
def distort_points_pinhole(ij):
# convert from pixel coordinates to metric
xy_ud = (ij - np.array([[cx, cy]])) / np.array([fx, fy])
x = xy_ud[:, 0]
y = xy_ud[:, 1]
r2 = np.sum(xy_ud**2, axis=1)
r4 = r2 ** 2
r6 = r4 * r2
xr = x * (1 + k1*r2 + k2*r4 + k3*r6)
yr = y * (1 + k1*r2 + k2*r4 + k3*r6)
xt = 2*p1*x*y + p2*(r2 + 2*x**2)
yt = p1*(r2 + 2*y**2) + 2*p2*x*y
# Convert to pixel coordinates
xd = fx * (xr + xt) + cx
yd = fy * (yr + yt) + cy
return np.vstack((xd, yd)).T
img = cv2.imread("test_data/dcam_img.png")
# undistort using manually created map
[X, Y] = np.meshgrid(np.arange(width), np.arange(height), indexing="xy")
ij = np.vstack([X.ravel(), Y.ravel()]).transpose()
pts = distort_points_pinhole(ij)
map1 = pts[:, 0].reshape(height, width).astype(np.float32)
map2 = pts[:, 1].reshape(height, width).astype(np.float32)
img_ud = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR)
# undistort using OpenCV (note that we compared to above, we're not using cv2.getOptimalNewCameraMatrix)
map1_cv, map2_cv = cv2.initUndistortRectifyMap(K, np.array(dcam_intr["D"]), None, K, size, cv2.CV_32FC1)
img_ud_cv = cv2.remap(img, map1_cv, map2_cv, interpolation=cv2.INTER_LINEAR)
#
# undistort and distort some points
#
# points in distorted image:
pts = np.array([[50,106], [358,164], [432, 420]])
for pt in pts:
cv2.circle(img, pt, radius=10, color=(0,255,0), thickness=2)
# using OpenCV function to undistort
pts_ud = cv2.undistortPoints(pts.astype(np.float32), K, np.array(dcam_intr["D"]))
pts_ud = (pts_ud * np.array([fx, fy]) + np.array([cx, cy])).reshape(-1,2)
# draw undistorted points
for pt in np.round(pts_ud).astype(int):
cv2.drawMarker(img_ud_cv, pt, (255, 0, 0), thickness=3)
cv2.drawMarker(img_ud, pt, (255, 0, 0), thickness=3)
# using our Python implementation to distort again
distorted_pts = distort_points_pinhole(pts_ud)
distorted_pts = np.round(distorted_pts).astype(int)
# draw "roundtrip" points (undistorted and distorted again)
for pt in distorted_pts:
cv2.drawMarker(img, pt, (255, 0, 0), thickness=3)
fig, ax = plt.subplots(1,3, figsize=(12,5))
ax[0].imshow(img)
ax[0].set_title("Original DCAM image")
ax[0].legend("Distorted points",loc="upper right")
ax[1].imshow(img_ud_cv)
ax[1].set_title("Undistorted w/ OpenCV map")
ax[2].imshow(img_ud)
ax[2].set_title("Undistorted w/ manual map")
plt.show()
Interactive Python Notebook
For developers looking to experiment with and further explore the distortion methods described above, we have provided a python notebook. This notebook includes the code listed in this post and allows you to run the un-distortion methods on your own images.
camera_undistortion.ipynb (897.3 KB)