鱼眼相机模型与去畸变实现
1.坐标系说明
鱼眼相机模型涉及到世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换关系。对于分析鱼眼相机模型,假定世界坐标系下的坐标点,经过外参矩阵的变换转到相机坐标系,相机坐标再经过内参转换到像素坐标,具体如下
进一步进行变换得到如下
坐标(i, j)
位置对应的就是无畸变图中的像素坐标。
那么在已知像素坐标时,根据上述表达式就能得到归一化的相机坐标 .实际计算时,可以用内参矩阵求逆也可以直接变换得到。
xw = K_matrix_inv.dot(np.array([i, j, 1], dtype=float))
x_d = xw[0]
y_d = xw[1]
x = float(i)
y = float(j)
x1 = (x - cx) / fx # 求出ud ==> x1
y1 = (y - cy) / fy # 求出vd ==> y1
# x == x1, y == y1
2.opencv实现
分析opencv鱼眼矫正最重要的函数是fisheye::initUndistortRectifyMap()
,它能得到map1矩阵。对应opencv-python,
map1, map2 = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, new_camera_matrix, (w, h), cv2.CV_32FC1)
map1是一个2通道矩阵,它在(i, j)处的二维向量元素(u, v) = (map1(i, j)[0], map1(i, j)[1])的意义如下:
将畸变图像中(u, v) = (map1(i, j)[0], map1(i, j)[1])的元素,复制到(i, j)处,就得到了无畸变图像。
opencv官方给出的实现过程如下:
3.去畸变理论分析
鱼眼相机的入射与反射示意图如下图所示。对于相机坐标系下有一点 ,如果按照针孔相机模型投影,则不存在畸变,像点为,发生畸变后的像点坐标为。
在图中,.
在上图中不妨假设 ,最终可以求得和 的比值(与 无关),从而可求得去畸变后的点坐标 以及入射角 . 这里的实际就是对应于的齐次坐标。
实际的鱼眼镜头因为各种原因并不会精确的符合投影模型,为了方便鱼眼相机的标定,一般取关于泰勒展开式的前5项来近似鱼眼镜头的实际投影函数。具体来说,该近似结果最早由
Juho Kannala 和 Sami S. Brandt在《A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses》论文中提出了一种一般的鱼眼模型,也是opencv和一般通常使用的模型,用入射角 的奇数次泰勒展开式来进行鱼眼模型的通用表示:
通常设置,使得相应的变化在后续的含高次项目中体现,由此得到
结合发生畸变后对应的归一化相机坐标,可以求出.
注意,这里的,根据示意图可知,无论采用通用模型还是等距投影模型,都严格存在如下
对于 ,则有:
实际计算过程,都是已知无畸变的像素坐标(i,j) 推导得到畸变后的像素坐标(u,v),再借助remap函数完成像素插值。当需要通过已知的畸变像素坐标反向投影得到无畸变点的像素时,也就是已知,采用上述关系得到 ,此时已知, 需要求出对应的。所以畸变矫正的本质问题是求解关于 的一元高次方程
常见求解一元高次方程的方法有二分法、不动点迭代、牛顿迭代法。这里采用牛顿迭代法求解。
令 ,
循环迭代直到(具体精度根据需要自行设置,比如设置阈值1e-6),或达到迭代次数上限。求得 之后,未畸变像点 的坐标满足
详见下文4.3代码。
4.代码实现
4.1 调用opencv
def undistort_imgs_fisheye(camera_matrix, dist_coeffs,img):
# 注意:OpenCV 没有直接提供逆畸变的函数,但我们可以使用 cv2.initUndistortRectifyMap 和 cv2.remap 来模拟
w = int(img.shape[1])
h = int(img.shape[0])
border_width = int(w/4)
border_height = int(h/4)
img_bordered = cv2.copyMakeBorder(img, border_height, border_height, border_width, border_width, cv2.BORDER_ISOLATED)
h_new, w_new = img_bordered.shape[:2]
new_camera_matrix1, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs[:4], (w_new, h_new), 0.5, (w, h))
# 计算去畸变和逆畸变的映射
map1, map2 = cv2.fisheye.initUndistortRectifyMap(camera_matrix, dist_coeffs[:4], np.eye(3), new_camera_matrix1, (w_new, h_new), cv2.CV_16SC2)
#根据CV_16SC2, map1此时是一个2通道的矩阵,每个点(i, j)都是一个2维向量, u = map1(i, j)[0], v= map1(i, j)[1],畸变图中坐标为(u, v)的像素点,在无畸变图中应该处于(i, j)位置。
undistort_img = cv2.remap(img_bordered, map1, map2, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
return undistort_img
4.2 表达式实现
def undistort_imgs_fisheye_equid(params_matrix, distort, img):
fx = params_matrix[0][0]
fy = params_matrix[1][1]
cx = params_matrix[0][2]
cy = params_matrix[1][2]
distortion_params = distort
kk = distortion_params
width = int(img.shape[1] * 1)
height = int(img.shape[0] * 1)
print("w is: {}, h is: {}".format(width,height))
mapx = np.zeros((width, height), dtype=np.float32)
mapy = np.zeros((width, height), dtype=np.float32)
for i in tqdm(range(0, width), desc="calculate_maps"):
for j in range(0, height):
x = float(i) #x是去畸变后的像素坐标
y = float(j) #y是去畸变后的像素坐标
a = (x - cx) / fx # x ==> a
b = (y - cy) / fy # y ==> b
r = np.sqrt(a**2 + b**2)
theta = np.arctan2(r,1)
rd = (1.0 *theta + kk[0] * theta**3 + kk[1] * theta**5 + kk[2] * theta**7 + kk[3] * theta**9)
scale = rd/r if r!=0 else 1.0
x2 = fx * a * scale + cx # width // 2
y2 = fy *b * scale + cy # height // 2
mapx[i, j] = x2
mapy[i, j] = y2
distorted_image = cv2.remap(
img,
mapx.T,
mapy.T,
interpolation=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
)
return distorted_image, params_matrix
4.3 反向投影
def diff(k2, k3, k4, k5, theta):
theta_2 = theta * theta
theta_4 = theta_2 * theta_2
theta_6 = theta_4 * theta_2
theta_8 = theta_6 * theta_2
rd_diff = 1 + 3 * k2 * theta_2 + 5 * k3 * theta_4 + 7 * k4 * theta_6 + 9 * k5 * theta_8
return rd_diff
def distort_theta(k2, k3, k4, k5, theta):
theta_2 = theta * theta
theta_3 = theta * theta_2
theta_5 = theta_3 * theta_2
theta_7 = theta_5 * theta_2
theta_9 = theta_7 * theta_2
theta_d = theta + k2 * theta_3 + k3 * theta_5 + k4 * theta_7 + k5 * theta_9
return theta_d
def newton_itor_theta(k2, k3, k4, k5, r_d):
theta = r_d
max_iter = 500
for i in range(max_iter):
diff_t0 = diff(k2, k3, k4, k5, theta)
f_t0 = distort_theta(k2, k3, k4, k5, theta) - r_d
theta = theta - f_t0 / diff_t0
if abs(f_t0) < 1e-6:
break
return theta
def distort_imgs_fisheye_new(params_matrix, distort, img):
undistorted_image = np.zeros((img.shape))
fx = params_matrix[0][0]
fy = params_matrix[1][1]
cx = params_matrix[0][2]
cy = params_matrix[1][2]
K_matrix_inv = np.linalg.inv(params_matrix)
width = int(img.shape[1] * 1)
height = int(img.shape[0] * 1)
mapx = np.zeros((width, height), dtype=np.float32)
mapy = np.zeros((width, height), dtype=np.float32)
for i in tqdm(range(0, width), desc="calculate_maps"):
for j in range(0, height):
xw = K_matrix_inv.dot(np.array([i, j, 1], dtype=float))
x_d = xw[0]
y_d = xw[1]
x = float(i)
y = float(j)
x1 = (x - cx) / fx # 求出ud ==> x1
y1 = (y - cy) / fy # 求出vd ==> y1
phi = np.arctan2(y_d, x_d)
r_d = np.sqrt(x_d ** 2 + y_d ** 2)
theta = newton_itor_theta(distort[0],distort[1],distort[2],distort[3],r_d)
r = np.tan(theta)
# r_d = np.tan(theta_d)
a = x_d * r/r_d
b = y_d * r/r_d
u = a*fx + cx
v = b*fy + cy
mapx[i, j] = u
mapy[i, j] = v
return mapx, mapy