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

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

鱼眼相机的入射与反射示意图如下图所示。对于相机坐标系下有一点 $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 = 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