鱼眼相机模型与去畸变实现

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 ==> x1y1 = (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 = distortkk = distortion_paramswidth  = 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 ==> ab = (y - cy) / fy  # y ==> br = 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.0x2 = fx * a * scale + cx # width // 2y2 = fy *b * scale + cy # height // 2mapx[i, j] = x2mapy[i, j] = y2distorted_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 * thetatheta_4 = theta_2 * theta_2theta_6 = theta_4 * theta_2theta_8 = theta_6 * theta_2rd_diff = 1 + 3 * k2 * theta_2 + 5 * k3 * theta_4 + 7 * k4 * theta_6 + 9 * k5 * theta_8return rd_diffdef distort_theta(k2, k3, k4, k5, theta):theta_2 = theta * thetatheta_3 = theta * theta_2theta_5 = theta_3 * theta_2theta_7 = theta_5 * theta_2theta_9 = theta_7 * theta_2theta_d = theta + k2 * theta_3 + k3 * theta_5 + k4 * theta_7 + k5 * theta_9return theta_ddef newton_itor_theta(k2, k3, k4, k5, r_d):theta = r_dmax_iter = 500for i in range(max_iter):diff_t0 = diff(k2, k3, k4, k5, theta)f_t0 = distort_theta(k2, k3, k4, k5, theta) - r_dtheta = theta - f_t0 / diff_t0if abs(f_t0) < 1e-6:breakreturn thetadef 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 ==> x1y1 = (y - cy) / fy  # 求出vd ==> y1phi = 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_db = y_d * r/r_du = a*fx + cxv = b*fy + cymapx[i, j] = umapy[i, j] = vreturn mapx, mapy