SLAM学习——相机模型（针孔+鱼眼）

针孔相机模型

针孔相机模型是很常用，而且有效的模型，它描述了一束光线通过针孔之后，在针孔背面投影成像的关系，基于针孔的投影过程可以通过针孔和畸变两个模型来描述。

模型中有四个坐标系，分别为world，camera，image，pixel

world为世界坐标系，可以任意指定 x w x_w xw轴和 y w y_w yw轴，为上图P点所在坐标系。
camera为相机坐标系，原点位于小孔， z c z_c zc轴与光轴重合， x c x_c xc轴和 y c y_c yc轴平行投影面，为上图坐标系 X c Y c Z c X_cY_cZ_c XcYcZc。
- 归一化坐标系，原点位于 z c = 1 z_c=1 zc=1，x和y轴与相机坐标系一样。
image为图像坐标系，原点位于光轴和投影面的交点， x x x轴和 y y y轴平行投影面，为上图坐标系 x y xy xy，相对于camera相差一个缩放。
pixel为像素坐标系（opencv定义），从小孔向投影面方向看，投影面的左上角为原点， u v uv uv轴和投影面两边重合，该坐标系与图像坐标系处在同一平面，但原点不同，相对于image相差一个平移。

投影变换

正投影（空间点->图像点）

world->camera

设点在world中的坐标为 P w = ( x w , y w , z w ) T P_w = (x_w,y_w,z_w)^T Pw=(xw,yw,zw)T，在camera中的坐标为 P c = ( x c , y c , z c ) T P_c = (x_c,y_c,z_c)^T Pc=(xc,yc,zc)T
[ P c 1 ] = T w c [ P w 1 ] \begin{bmatrix} P_c\\1 \end{bmatrix}= T^c_w \begin{bmatrix} P_w\\1 \end{bmatrix} [Pc1]=Twc[Pw1]

其中 T w c T^c_w Twc为外参，即世界坐标系相对于相机坐标系的变换。
camera->归一化相机系

相机坐标系为 F s F_s Fs，设一平面位于相机坐标camera的 z = 1 z=1 z=1上，为归一化平面，其坐标系 C C C为归一化坐标系。

根据相似三角形，P点在归一化坐标系的坐标为
P n = [ x c / z c y c / z c z c / z c ] = [ x n y n 1 ] P_n = \begin{bmatrix} x_c/z_c\\y_c/z_c\\z_c/z_c \end{bmatrix} =\begin{bmatrix} x_n\\y_n\\1 \end{bmatrix} Pn= xc/zcyc/zczc/zc = xnyn1
归一化相机系->camera->pixel

设置图像坐标系上坐标为 P i P_i Pi，基于图中相似三角形（一般所得图像已经经过处理，完成翻转）可得点P在图像坐标系上坐标为：
x i f = x c z c = x n , y i f = y c z c = y n \frac{x_i}{f} = \frac{x_c}{z_c} = x_n, \frac{y_i}{f} = \frac{y_c}{z_c} = y_n fxi=zcxc=xn,fyi=zcyc=yn

设 α \alpha α和 β \beta β分别为x和y方向上的每米像素值，设图像中心偏移为 [ c x , c y ] [c_x,c_y] [cx,cy]，设像素坐标 P u v = ( u , v , 1 ) T P_{uv} = (u,v,1)^T Puv=(u,v,1)T，则
u = α x i + c x = α f x n + c x , v = β y i + c y = β f y n + c y u = \alpha x_i+c_x = \alpha f x_n +c_x,v = \beta y_i+c_y = \beta f y_n +c_y u=αxi+cx=αfxn+cx,v=βyi+cy=βfyn+cy

设 f x = α f , f y = β f f_x = \alpha f, f_y = \beta f fx=αf,fy=βf，即
P u v = [ f x 0 c x 0 f y c y 0 0 1 ] P n = K P n P_{uv} = \begin{bmatrix} f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix}P_n = K P_n Puv= fx000fy0cxcy1 Pn=KPn

其中K为内参数矩阵(Camera Intrinsics)。

因此world到pixel到关系为
P u v = K 3 × 3 T W C P w P_{uv}=K_{3 \times 3}T^C_WP_w Puv=K3×3TWCPw

其中隐含了一次齐次坐标到非齐次坐标的转换，具体坐标系变换流程为
世界坐标系 → 相机坐标系 ( 归一化处理 ) → 图像坐标系 → 像素坐标系 w o r l d → c a m r e a → i m a g e → p i x e l P w = ( x w , y w , z w , 1 ) T → P c = ( x c , y c , z c , 1 ) T [ P i = ( x i , y i , 1 ) T ] → P u v = ( u , v , 1 ) T 世界坐标系\rightarrow 相机坐标系( 归一化处理)\rightarrow 图像坐标系 \rightarrow 像素坐标系\\ world \rightarrow camrea \rightarrow image \rightarrow pixel\\ P_w = (x_w,y_w,z_w,1)^T\rightarrow P_c = (x_c,y_c,z_c,1)^T [P_i = (x_i,y_i,1)^T ]\rightarrow P_{uv} = (u,v,1)^T 世界坐标系→相机坐标系(归一化处理)→图像坐标系→像素坐标系world→camrea→image→pixelPw=(xw,yw,zw,1)T→Pc=(xc,yc,zc,1)T[Pi=(xi,yi,1)T]→Puv=(u,v,1)T

cpp 复制代码

//Parameters vector corresponds to
//      [fx, fy, cx, cy]
// 归一化平面->pixel
Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) {
    Eigen::Vector2d res;
    res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
    res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];

    return res;
}

反投影（图像点->空间点）

设已知图像点在相机坐标系下的深度为Z
P w = T c w Z K − 1 P u v P_w = T^w_c Z K^{-1} P_{uv} Pw=TcwZK−1Puv

cpp 复制代码

//Parameters vector corresponds to
//      [fx, fy, cx, cy]
// pixel->归一化平面
cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) {
    return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1],1.f);
}

畸变

一般使用的去畸变处理方法：先对整张图像去畸变，得到去畸变后的图像，然后讨论图像上点的空间位置。
{ x c o r r e c t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y c o r r e c t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{cases} x_{corrected} =x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2) \\ y_{corrected} =y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{cases} {xcorrected=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ycorrected=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy

上式为对归一化平面上的点进行径向畸变和切向畸变纠正，其中k1、k2、k3为径向畸变，p1、p2为切向畸变，根据需求可以调整使用的参数，不一定要五个全用上。

径向畸变

径向畸变（桶形畸变和枕形畸变）产生原因：光线在远离透镜中心的地方偏折更大，矫正公式：
{ x c o r r e c t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y c o r r e c t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) \begin{cases} x_{corrected} =x(1+k_1r^2+k_2r^4+k_3r^6) \\ y_{corrected} =y(1+k_1r^2+k_2r^4+k_3r^6) \end{cases} {xcorrected=x(1+k1r2+k2r4+k3r6)ycorrected=y(1+k1r2+k2r4+k3r6)

切向畸变

切向畸变产生原因：相机组装过程不能使透镜和成像平面严格平行引起，矫正公式：
{ x c o r r e c t e d = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y c o r r e c t e d = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{cases} x_{corrected} =x+2p_1xy+p_2(r^2+2x^2) \\ y_{corrected} =y+p_1(r^2+2y^2)+2p_2xy \end{cases} {xcorrected=x+2p1xy+p2(r2+2x2)ycorrected=y+p1(r2+2y2)+2p2xy

小结

综上，16个单目相机的参数：

10个内部参数（内参，只与相机有关）：
- 5个内部矩阵参数K： f , d x , d y , u 0 , v 0 f,d_x,d_y,u_0,v_0 f,dx,dy,u0,v0（也可视作4个参数 f x , f y , u 0 , v 0 f_x,f_y,u_0,v_0 fx,fy,u0,v0）
- 5个畸变参数D： k 1 , k 2 , k 3 , p 1 , p 2 k_1,k_2,k_3,p_1,p_2 k1,k2,k3,p1,p2
6个外部参数（外参，描述目标点的世界坐标到像素坐标的投影关系）：
- 3个旋转参数R
- 3个平移参数t

鱼眼相机模型

鱼眼相机与针孔相机原理不同，采用非相似成像，在成像过程中引入畸变，通过对直径空间的压缩，突破成像视角的局限，从而达到广角成像，所以鱼眼镜头是一种极端得广角镜头，通常焦距小于等于16mm并且视角接近或等于180°（在工程上视角超过140°的镜头即统称为鱼眼镜头）。

鱼眼相机成像模型近似为单位球面投影模型，一般将鱼眼相机成像过程分解成两步：

先将三维空间点线性的投影到虚拟单位球面上
随后将单位球面上的点投影到图像平面上，这个过程是非线性的

由于鱼眼相机所成影像存在畸变，其中径向畸变非常严重，因此其畸变模型主要考虑径向畸变。

鱼眼相机的投影函数是为了尽可能将庞大的场景投影到有限的图像平面所设计的。根据投影函数的不同，鱼眼相机的设计模型大致分为

等距投影
r d = f θ r_d = f \theta rd=fθ
等立体角投影
r d = 2 f sin ⁡ ( θ 2 ) r_d = 2 f \sin(\frac{\theta}{2}) rd=2fsin(2θ)
正交投影
r d = 2 f sin ⁡ ( θ ) r_d = 2 f \sin(\theta) rd=2fsin(θ)
体视投影
r d = 2 f tan ⁡ ( θ ) r_d = 2 f \tan(\theta) rd=2ftan(θ)

相机的成像模型实际上表征的是成像的像高与入射角之间的映射关系。

通用鱼眼相机模型-Kannala-Brandt

Kannala-Brandt模型主要用于第二步，单位球面上的点投影到图像平面，该步骤为非线性，不同投影可以统一为
r d = f θ d r_d =f\theta _d rd=fθd

因sin，tan的泰勒展开都是奇次项形式 ，取前5项可得
θ d = k 0 θ + k 1 θ 3 + k 2 θ 5 + k 3 θ 7 + k 4 θ 9 \theta_d= k_0\theta + k_1 \theta^3 + k_2 \theta^5 + k_3 \theta^7 + k_4 \theta^9 θd=k0θ+k1θ3+k2θ5+k3θ7+k4θ9

正投影（空间点->图像点）

世界系->归一化相机系

设外参为 T w c T^c_w Twc
[ X c Y c Z c ] = R [ X w Y w Z w ] + t x c = X c Z c , y c = Y c Z c \left[ \begin{array}{c} X_c \\ Y_c \\ Z_c \end{array} \right]=R \left[\begin{array}{c} X_w \\ Y_w \\ Z_w \end{array}\right]+t\\ x_c = \frac{X_c}{Z_c},\ y_c=\frac{Y_c}{Z_c} XcYcZc =R XwYwZw +txc=ZcXc, yc=ZcYc
归一化相机系->虚拟单位球面

r 2 = x c 2 + y c 2 θ = a r c t a n ( r ) θ d = k 0 θ + k 1 θ 3 + k 2 θ 5 + k 3 θ 7 + k 4 θ 9 x d = θ d r x c , y d = θ d r y c r^2 = x^2_c + y^2_c\\ \theta = arctan(r)\\ \theta _d = k_0\theta + k_1 \theta^3 + k_2 \theta^5 + k_3 \theta^7 + k_4 \theta^9\\ x_d=\frac{\theta_d}{r}x_c, \ y_d=\frac{\theta_d}{r}y_c r2=xc2+yc2θ=arctan(r)θd=k0θ+k1θ3+k2θ5+k3θ7+k4θ9xd=rθdxc, yd=rθdyc

虚拟单位球面->图像系
u = f x x d + c x , v = f y y d + c y u=f_xx_d+c_x, \ v=f_yy_d+c_y u=fxxd+cx, v=fyyd+cy

变换后为拉伸变形图像。

cpp 复制代码

// Parameters vector corresponds to
// [fx, fy, cx, cy, k0, k1, k2, k3]    
// 相机系->图像系
Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) {
    const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
    const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
    const float psi = atan2f(v3D[1], v3D[0]);

    const float theta2 = theta * theta;
    const float theta3 = theta * theta2;
    const float theta5 = theta3 * theta2;
    const float theta7 = theta5 * theta2;
    const float theta9 = theta7 * theta2;
    const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                    + mvParameters[6] * theta7 + mvParameters[7] * theta9;

    Eigen::Vector2f res;
    res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
    res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

    return res;
}

反投影（图像点->空间点）

图像系->虚拟单位球面

x d = u − c x f X , y d = v − c y f y x_d = \frac{u-c_x}{f_X},y_d = \frac{v-c_y}{f_y} xd=fXu−cx,yd=fyv−cy
虚拟单位球面->归一化相机系->相机系
θ d = x d 2 + y d 2 \theta _d = \sqrt{x_d^2+y_d^2} θd=xd2+yd2

使用牛頓法 (Newton's method)求解 θ \theta θ
θ = N e w t o n ( θ d . k 1 , k 2 , k 3 , k 4 ) \theta = Newton(\theta_d.k_1,k_2,k_3,k_4) θ=Newton(θd.k1,k2,k3,k4)

x c = tan ⁡ θ θ d ∗ x d , y c = tan ⁡ θ θ d ∗ y d x_c =\frac{\tan{\theta}}{\theta_d} * x_d, y_c =\frac{\tan{\theta}}{\theta_d} * y_d xc=θdtanθ∗xd,yc=θdtanθ∗yd

设相机系下深度为Z
P c = [ X c Y c Z c ] = Z [ x c y c 1 ] P_c = \left[ \begin{array}{c} X_c \\ Y_c \\ Z_c \end{array} \right] = Z \left[ \begin{array}{c} x_c \\ y_c \\ 1 \end{array} \right] Pc= XcYcZc =Z xcyc1

相机系->世界系
P w = T c w P c P_w = T^w_c P_c Pw=TcwPc

cpp 复制代码

// Parameters vector corresponds to
// [fx, fy, cx, cy, k0, k1, k2, k3]
// 图像系->归一化相机系
cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
    //Use Newthon method to solve for theta with good precision (err ~ e-6)
    cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]);
    float scale = 1.f;
    float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y);
    theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f);

    if (theta_d > 1e-8) {
        //Compensate distortion iteratively
        float theta = theta_d;

        for (int j = 0; j < 10; j++) {
             float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 =
                   theta4 * theta4;
             float k0_theta2 = mvParameters[4] * theta2, k1_theta4 = mvParameters[5] * theta4;
             float k2_theta6 = mvParameters[6] * theta6, k3_theta8 = mvParameters[7] * theta8;
             float 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 (fabsf(theta_fix) < precision)
                 break;
         }
         //scale = theta - theta_d;
         scale = std::tan(theta) / theta_d;
    }

    return cv::Point3f(pw.x * scale, pw.y * scale, 1.f);
}