Undistortion¶
Kevin Walcko
StereoCamera has a method that will give you a UndistortStereo object which allows you to undistort stereo images easily.
In [1]:
# reload library
%load_ext autoreload
%autoreload 2
In [27]:
import numpy as np
import cv2
from matplotlib import pyplot as plt
from opencv_camera import StereoCamera
from opencv_camera import visualizeDistortion
from opencv_camera import UndistortStereo, UnDistort
np.set_printoptions(precision=3)
np.set_printoptions(suppress=True)
In [3]:
img = cv2.imread("aruco-imgs-2/0.png",0)
h,w = img.shape[:2]
imgl = img[:,:w//2]
imgr = img[:,w//2:]
In [5]:
plt.imshow(np.hstack((imgl, imgr)), cmap="gray")
plt.axis("off");
In [25]:
sc = StereoCamera.from_yaml("camera.yml")
print(sc)
Camera 1 -------------------------- focalLength(x,y): 1061.4 1060.8 px principlePoint(x,y): 626.2 368.0 px distortionCoeffs: [[ 0.123 -0.545 0. 0.001 0.657]] Camera 2 -------------------------- focalLength(x,y): 1059.1 1058.6 px principlePoint(x,y): 629.4 368.4 px distortionCoeffs: [[ 0.121 -0.528 0.002 -0.002 0.634]] Extrinsic Camera Parameters ------- Translation between Left/Right Camera: [[-0.03 -0. -0. ]] Rotation between Left/Right Camera: [[ 1. 0.001 0. ] [-0.001 1. -0.002] [-0. 0.002 1. ]] Essential Matrix: [[-0. 0. -0. ] [-0. 0. 0.03] [ 0. -0.03 0. ]] Fundatmental Matrix: [[-0. 0. -0.021] [-0. 0. 2.542] [ 0.025 -2.54 1. ]]
In [26]:
h,w = imgl.shape[:2]
uds = sc.getUndistortion(h,w)
a, b = uds.undistort(imgl, imgr)
plt.figure(figsize=(10,5))
plt.imshow(np.hstack((imgl, imgr)), cmap="gray");
plt.axis("off")
plt.title("raw");
plt.figure(figsize=(10,5))
plt.imshow(np.hstack((a, b)), cmap="gray");
plt.axis("off")
plt.title("undistorted");
Appendix: Source Code¶
In [30]:
from jtb import getCodeUrl, getCodeFile, getCodeImport
getCodeImport(UnDistort)
class UnDistort:
def __init__(self, K, d, h, w, R=None):
"""
Sets up the class with an Optimal Camera Matrix alpha of zero, which
removes all unwanted pixels
camMat: camera matrix
dist: distortion coefficients from calibration
w: width
h: height
R: the rotation matrix from cv2.stereoCalibration(), this is optional.
For undistorting stereo images, only use R on the right camera image
and not the left one.
"""
self.K = K
self.d = d
self.size = (w,h) # backwards
self.shape = (h,w)
optCamMat, _ = cv2.getOptimalNewCameraMatrix(K, d, self.size, 0)
self.R = R
self.mapx, self.mapy = cv2.initUndistortRectifyMap(
K,d,
R,
optCamMat,
self.size, # (w,h) -- backwards # (w,h) -- backwards
cv2.CV_32FC1
)
def undistort(self, image, alpha=None):
"""
image: an image
alpha: values between 0 and 1 which determines the amount of unwanted
pixels. The default is 0, but if changed, a new Optimal Camera
Matrix is calculated for the alpha
alpha = 0: returns undistored image with minimum unwanted pixels (image
pixels at corners/edges could be missing)
alpha = 1: retains all image pixels but there will be black to make up
for warped image correction
Note: this is about 5x faster than using cv2.undistort(), BUT the
self.mapx and self.mapy are EACH the same size as the image and both
are float32. So although faster, it consumes more memory.
"""
if self.shape != image.shape:
raise Exception(f"Undistort set for image.shape = {self.shape}, not {image.shape}")
if alpha is not None:
optCamMat, _ = cv2.getOptimalNewCameraMatrix(
self.K,
self.d,
self.size, # (w,h) -- backwards
alpha
)
self.mapx, self.mapy = cv2.initUndistortRectifyMap(
self.K, self.d, self.R,
optCamMat,
self.size, # (w,h) -- backwards
cv2.CV_32FC1)
return cv2.remap(image,self.mapx,self.mapy,cv2.INTER_LINEAR)
# def distortionMap(self):
# """
# Return a numpy array representing the image distortion
# """
# raise NotImplemented()
In [29]:
getCodeImport(UndistortStereo)
class UndistortStereo:
def __init__(self, K1, d1, K2,d2, h, w, R):
self.left = UnDistort(K1,d1,h,w)
self.right = UnDistort(K2,d2,h,w,R)
def undistort(self, a, b):
a = self.left.undistort(a)
b = self.right.undistort(b)
return a,b
In [ ]: