【OPENCV源码阅读】鱼眼镜头标定

Opencv的鱼眼镜头模型

• 空间中的三维点P，在世界坐标系下为X，在相机坐标系下坐标为$X_c$，R是旋转对阵。
  $Xc=RX+T$

• x，y，z是$X_c$的三个坐标值
  $x=X_{c1}$
  $y=X_{c2}$
  $z=X_{c3}$

• 三维点P在相机归一化平面上的投影p的坐标为(a,b)
  $a=x/z$ and $b=y/z$

• 其在镜头上的入射角θ为
  $r^2=a^2+b^2$
  $\theta =atan(r)$
  θ是入射角（入射光与光轴夹角），r是归一化平面上p到平面中心距离。

• 鱼眼畸变模型，注意${\theta }_d$不是角度而是r畸变后的长度。
  $r_d=\theta (1+k_1{\theta }_2+k_2{\theta }_4+k_3{\theta }_6+k_4{\theta }_8)$

• 畸变点坐标 [x’; y’]为：
  $x\prime =(r_d/r)\cdot a$
  $y\prime =(r_d/r)\cdot b$

• 最后乘焦距映射成像素坐标 [u; v]:
  $u=f_x(x\prime +\alpha y\prime )+c_x$
  $v=f_{y}y\prime +c_y$

去畸变（到小孔成像的变换）

图像去畸变

cv::fisheye::initUndistortRectifyMap (InputArray K, InputArray D, InputArray R, InputArray P, const cv::Size &size, int m1type, OutputArray map1, OutputArray map2)
用后向映射的方式，计算目的图像上每个点对应源图像的位置。

点去畸变

void cv::fisheye::undistortPoints (InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R=noArray(), InputArray P=noArray())

这个要用前向映射的方式，计算源图像上点在目的图像上的位置。这涉及一个问题，需要利用$r_d$求解${\theta }_d$。Opencv使用的是最优化的方法，利用牛顿法求入射角:

目标函数: $f(\theta )=\theta (1+k_1{\theta }_2+k_2{\theta }_4+k_3{\theta }_6+k_4{\theta }_8)-{\theta }_d=0$
方向和步长为：$-f(\theta )/f^{\prime }(\theta )$

这是Opencv的实现代码，最大迭代次数为10次。

            for (int j = 0; j < 10; j++)
            {
                double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2;
                double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8;
                /* new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */
                double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
                                   (1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8);
                theta = theta - theta_fix;
                if (fabs(theta_fix) < EPS)
                    break;
            }

初始值如下，即取$\theta ={\theta }_d$

min(max(-CV_PI/2., theta_d), CV_PI/2.);