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

1.坐标系说明

鱼眼相机模型涉及到世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换关系。对于分析鱼眼相机模型，假定世界坐标系下的坐标点 $P_W$ $(x_w,y_w,z_w)$ ,经过外参矩阵的变换转到相机坐标系 $P(x_c,y_c,z_c)$ ，相机坐标再经过内参转换到像素坐标，具体如下

进一步进行变换得到如下

坐标(i, j)位置对应的就是无畸变图中的像素坐标。

那么在已知像素坐标时，根据上述表达式就能得到归一化的相机坐标 $(\bar{x_c},\bar{y_c},1)$ .实际计算时，可以用内参矩阵求逆也可以直接变换得到。

 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.去畸变理论分析

鱼眼相机的入射与反射示意图如下图所示。对于相机坐标系下有一点 $P(x,y,z)$ ,如果按照针孔相机模型投影，则不存在畸变，像点为 $P_0(a,b)$ ，发生畸变后的像点坐标为 $p'$ 。

在图中， $r=\parallel OP_0\parallel,r_d=\parallel Op'\parallel$ .

在上图中不妨假设 $f = 1$ ，最终可以求得 $r_d$ 和 $r$ 的比值（与 $f$ 无关）,从而可求得去畸变后的 $P_0$ 点坐标 $(a,b)$ 以及入射角 $\theta$ . 这里的 $(a,b,1)$ 实际就是对应于 $P_0$ 的齐次坐标。

实际的鱼眼镜头因为各种原因并不会精确的符合投影模型，为了方便鱼眼相机的标定，一般取 $r_d$ 关于 $\theta$ 泰勒展开式的前5项来近似鱼眼镜头的实际投影函数。具体来说,该近似结果最早由

Juho Kannala 和 Sami S. Brandt在《A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses》论文中提出了一种一般的鱼眼模型，也是opencv和一般通常使用的模型，用入射角 $\theta$ 的奇数次泰勒展开式来进行鱼眼模型的通用表示：

$r_d =\theta(k_0+k_1\theta^2+k_2\theta^4+k_3\theta^6+k_4\theta^8)$

通常设置 $k_0=1$ ，使得相应的变化在后续的含 $k_1,k_2,k_3,k_4$ 高次项目中体现，由此得到

$r_d =\theta(1+k_1\theta^2+k_2\theta^4+k_3\theta^6+k_4\theta^8)$

结合发生畸变后对应的归一化相机坐标 $(x',y',1)$ ，可以求出 $(a,b)$ .

$\left\{\begin{matrix} u=f_xx'+c_x \\ \\ v=f_yy'+c_y \end{matrix}\right. \Rightarrow \left\{\begin{matrix} x'=(u-c_x)/f_x \\ \\ y'=(v-c_y)/f_y\end{matrix}\right.$

$\left\{\begin{matrix} \frac{a}{r}= \frac{x'}{r_d} \\ \\ \frac{b}{r}= \frac{y'}{r_d}\end{matrix}\right.\Rightarrow \left\{\begin{matrix} a=\frac{r}{r_d}x' \\ \\ b=\frac{r}{r_d}y' \end{matrix}\right.$

注意，这里的 $r_d\neq \theta_d$ ,根据示意图可知,无论采用通用模型还是等距投影模型，都严格存在如下

$\left\{\begin{matrix} tan(\theta)= \frac{OP_0}{f}=\frac{r}{f} \\ \\ tan(\theta_d)= \frac{Op'}{f}=\frac{r_d}{f}\end{matrix}\right.$

对于 $f=1$ ,则有：

$\left\{\begin{matrix}\theta = arctan(r) \\ \\ \theta_d = arctan(r_d)\end{matrix}\right.$

实际计算过程，都是已知无畸变的像素坐标(i,j) 推导得到畸变后的像素坐标（u，v），再借助remap函数完成像素插值。当需要通过已知的畸变像素坐标反向投影得到无畸变点的像素时，也就是已知 $(u,v)$ ,采用上述关系得到 $(x',y')$ ，此时已知 $r_d$ ，需要求出对应的 $\theta$ 。所以畸变矫正的本质问题是求解关于 $\theta$ 的一元高次方程

$r_d =\theta(1+k_1\theta^2+k_2\theta^4+k_3\theta^6+k_4\theta^8)$

常见求解一元高次方程的方法有二分法、不动点迭代、牛顿迭代法。这里采用牛顿迭代法求解。

令 $f(\theta) =\theta+k_1\theta^3+k_2\theta^5+k_3\theta^7+k_4\theta^9-r_d$ ,

$\theta_0=r_d$

$\theta_{n+1}=\theta_{n}-\frac{f(\theta_n)}{f'(\theta_{n})}$

循环迭代直到 $f(\theta)\approx 0$ （具体精度根据需要自行设置,比如设置阈值1e-6），或达到迭代次数上限。求得 $\theta$ 之后，未畸变像点 $P_0$ 的坐标满足

$\left\{\begin{matrix} a=\frac{r}{r_d}x' \\ \\ b=\frac{r}{r_d}y' \end{matrix}\right.$

详见下文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