【OPENCV源碼閱讀】魚眼鏡頭標定

Opencv的魚眼鏡頭模型

空間中的三維點，對應世界座標X，相機座標系下座標爲$X_c$

$Xc=RX+T$
R是旋轉對陣，對應於旋轉向量om；x，y，z是$X_c$的三個座標值

$x=X_{c1}$
$y=X_{c2}$
$z=X_{c3}$

針孔成像模型下P的座標(a,b)（歸一化平面座標）
$a=x/z$ and $b=y/z$
$r^2=a^2+b^2$
$θ=atan(r)$
θ是入射角（入射光與光軸夾角），r是歸一化平面像點到平面光心距離。

魚眼畸變模型，注意$θ_d$不是角度而是r畸變後的長度。
$θ_d=θ(1+k_1θ_2+k_2θ_4+k_3θ_6+k_4θ_8)$

畸變點座標 [x’; y’]爲：
$x′=(θ_d/r)a$
$y′=(θ_d/r)b$

最後乘焦距映射成像素座標 [u; v]:
$u=f_x(x′+αy′)+c_x$
$v=f_yy′+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())

這個要用前向映射的方式，這樣就會產生一個問題，得到$θ_d$後沒法直接求解出入射角度$θ$或r。Opencv使用的是最優化的方法，利用牛頓法求入射角:

目標函數: $f(θ)=θ(1+k_1θ_2+k_2θ_4+k_3θ_6+k_4θ_8)-θ_d=0$
方向和步長爲：$-f(θ)/f'(θ)$

這是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;
            }

初始值如下，即取$θ=θ_d$

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