1、 成像投影原理
其成像过程分解成两步：
１．归一化平面上（Zc = 1）的三维空间点线性地投影到一个球面上，它是一个虚拟的单位球面，它的球心与相机坐标系的原点重合；
２．单位球面上的点投影到图像平面上，这个过程是非线性的，并产生畸变。
等距模型：
投影模型描述： 鱼眼图像中的点到畸变中心的距离 r_d 与投影角度 theta 的关系。
投影模型与畸变没有关系，即使没有畸变发生，也是按照这种方式进行投影。
等距投影模型：
r_d 为入射线与虚拟球面的交点到Z轴的距离。由于畸变的影响，r_d 在径向上有所调整，但是不改变三角相似性。即:
2、 成像推导过程
2-1、世界坐标到相机坐标系
R、T 变换将空间中一点转换到鱼眼坐标下。
归一化到鱼眼Zc = 1 平面上。得到归一化（x, y，1）
2-2、归一化平面到单位球面
根据映射点（x, y，1）投影到虚拟球面上，求 theta ， theta 为入射光线与 Zc 轴夹角。
2-3、theta 畸变
加入畸变系数，求畸变theta_d。
2-4、等比映射
根据 : r_d = f * theta
f 焦距固定之下， r_d 与 theta 成等比关系。
r_d1 / r_d2 = theta1 / theta2
所以 theta 的 畸变角度 为 theta_d 时:
r / r_d = theta / theta_d
即可重新求得畸变后的相机坐标系下的（xd, yd), 上面的过程始终在3维空间中。
2-5、相机坐标系到成像平面
3、 单点去畸变
在鱼眼图像上获取像素点 (u，v) , 经过内参反映射到相机三维坐标归一化面上，此时得到的点是经过畸变后的点（x_d, y_d）,而我们希望获取的是没有畸变的三维空间坐标点（x_c, y_c）
在相机坐标系中有：
所以等距模型中：
已知 theta_d 和畸变参数k2, k3, k4, k5。就可以通过迭代法求解 theta。
其中牛顿迭代代码如下：
def gf(self, k2, k3, k4, k5, theta):theta_2 = theta * thetatheta_4 = theta_2 * theta_2theta_6 = theta_4 * theta_2theta_8 = theta_6 * theta_2gf = 1 + 3 * k2 * theta_2 + 5 * k3 * theta_4 + 7 * k4 * theta_6 + 9 * k5 * theta_8return gfdef distort_theta(self, 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(self, k2, k3, k4, k5, theta_d):theta = theta_dmax_iter = 10for i in range(max_iter):gf_t0 = self.gf(k2, k3, k4, k5, theta)f_t0 = self.distort_theta(k2, k3, k4, k5, theta) - theta_dtheta = theta - f_t0 / gf_t0if abs(f_t0) < 1e-6:breakreturn theta已知经纬度角度后，映射到球面坐标上： 的到相机坐标系真实三维空间坐标。
映射到实际成像 Z = R 平面上。
反投影映射代码如下：
def lift_projective(self, p):xw = self.K_matrix_inv.dot(np.array([p[0], p[1], 1], dtype=float))x_d = xw[0]y_d = xw[1]phi = np.arctan2(y_d, x_d)theta_d = np.sqrt(x_d ** 2 + y_d ** 2)theta = self.newton_itor_theta(self.k2, self.k3, self.k4, self.k5, theta_d)x_c = np.sin(theta) * np.cos(phi)y_c = np.sin(theta) * np.sin(phi)z_c = np.cos(theta)return np.array([x_c, y_c, z_c])
