相机模型与畸变处理

针孔相机

SLAM中采用的相机模型大多基于针孔相机模型，在说明针孔相机模型的投影关系之前，先明确需要用到的几个坐标系下用一个空间点的不同坐标表示：

-世界坐标系$w$：空间点在世界坐标系下的坐标表示为${\mathbf{P}}^w={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T$ ；

• 相机坐标系$c$：空间点在相机坐标系下的坐标表示为${\mathbf{p}}^c={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T$ ；
• 像素坐标系$p$：空间点在像素坐标系下的坐标表示为${\mathbf{u}}^p={\left[\begin{array}{cc}u& v\end{array}\right]}^T$ ；

空间点在世界坐标系和相机坐标系下的关系由外参矩阵$\mathbf{T}$决定，注意由于$\mathbf{T}$是4维矩阵，这里所有坐标需采用齐次表示：
$$\mathbf{p}=\mathbf{T}\mathbf{P}$$
记空间点在成像平面上的坐标为${{\mathbf{p}}^c}^{\prime }={\left[\begin{array}{ccc}{x}^{\prime }& y^{\prime }& z^{\prime }\end{array}\right]}^T$，则有：
$$\{\begin{array}{rl}{x}^{\prime }& =\frac{f}{z}x\\ y^{\prime }& =\frac{f}{z}y\end{array}$$
由于像素坐标系和成像坐标系原点不一致（成像坐标系原点位于光心延长线，像素坐标系原点位于图像左上角），记两者之间的坐标平移量为$\left[\begin{array}{cc}{c}_x& c_y\end{array}\right]$；同时，成像坐标系单位为$m$，像素坐标系单位为像素$pixel$，记两者之间的转换比例为$\left[\begin{array}{cc}\alpha & \beta \end{array}\right]$，则像素坐标和相机坐标之间的关系可以表示为：
$$\{\begin{array}{rl}u& =\alpha \frac{f}{z}x+c_x\\ v& =\beta \frac{f}{z}y+c_y\end{array}$$
令$\alpha f=f_x$、$\beta f=f_y$，同时将上式写成齐次矩阵形式为：
$$\frac{1}{z}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\underset{\mathbf{K}}{\underbrace{\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$
矩阵$\mathbf{K}$称为相机的内参矩阵。

畸变模型

径向畸变模型

上式的空间点投影实在理想状态下给出的，实际情况下的成像存在由各种原因引起的畸变，需要我们对其进行进一步矫正。所有畸变中，最常见且影响最大的为径向畸变和切向畸变。径向畸变采用多项式模型描述，通常我们只取其前两项，即：
$$\{\begin{array}{rl}{x}_{distorted}& =x\left(1+k_1{r}^2+k_2{r}^4\right)\\ y_{distorted}& =y\left(1+k_1{r}^2+k_2{r}^4\right)\\ r^2& =x^2+y^2\end{array}$$

切向畸变模型

相比于径向畸变模型，切向畸变模型则更为复杂：
$$\{\begin{array}{rl}{x}_{distorted}& =x+\left[2p_{1}xy+p_2\left(r^2+2x^2\right)\right]\\ y_{distorted}& =y+\left[p_1\left(r^2+2y^2\right)+2p_{2}xy\right]\\ r^2& =x^2+y^2\end{array}$$

联合畸变模型

综上，我们可以得到同时存在径向和切向畸变的相机模型为：
$$\{\begin{array}{rl}{x}_{distorted}& =x\left(1+k_1{r}^2+k_2{r}^4\right)+\left[2p_{1}xy+p_2\left(r^2+2x^2\right)\right]\\ y_{distorted}& =y\left(1+k_1{r}^2+k_2{r}^4\right)+\left[p_1\left(r^2+2y^2\right)+2p_{2}xy\right]\\ r^2& =x^2+y^2\end{array}$$

畸变矫正方法

在相机各项畸变参数已知的情况下，由于$r^2$的存在，想要直接求解上式也是困难的，通常我们采用迭代方式求解，另畸变方程左边等于零有
$$\begin{array}{rl}& \{\begin{array}{rl}& x\left(1+k_1{r}^2+k_2{r}^4\right)=x_{distorted}-\left[2p_{1}xy+p_2\left(r^2+2x^2\right)\right]\\ & y\left(1+k_1{r}^2+k_2{r}^4\right)=y_{distorted}-\left[p_1\left(r^2+2y^2\right)+2p_{2}xy\right]\end{array}\\ & \Rightarrow \{\begin{array}{rl}x& =\frac{x_{distorted}-\left[2p_{1}xy+p_2\left(r^2+2x^2\right)\right]}{\left(1+k_1{r}^2+k_2{r}^4\right)}\\ y& =\frac{y_{distorted}-\left[p_1\left(r^2+2y^2\right)+2p_{2}xy\right]}{\left(1+k_1{r}^2+k_2{r}^4\right)}\end{array}\end{array}$$
注意公式中的$x_{distorted}$和$y_{distorted}$为迭代初值，即带畸变的点坐标；$x$和$y$为当前迭代的去畸变点坐标。使用上式不断迭代，直至两步之间的迭代误差小于一定阈值即可，一个简单的demo如下：

/*
 * @Description: 
 * @Author: Yuntian Li
 * @Github: https://github.com/yuntinali91
 * @Date: 2019-09-24 14:28:08
 * @LastEditors: Yuntian Li
 * @LastEditTime: 2019-09-25 15:13:20
 */
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

using namespace std;

void distortion(const Eigen::Vector2d& u_u, Eigen::Vector2d& u_d, 
				const Eigen::Matrix3d& K, const Eigen::Vector4d& D);
void undistortion(const Eigen::Vector2d& u_u, Eigen::Vector2d& u_d, 
				const Eigen::Matrix3d& K, const Eigen::Vector4d& D, const double tolerance);

int main(int argc, char** argv){
    // distortion coefficients
    Eigen::Vector4d D{-2.917e-1, 8.228e-2, 5.333e-5, -1.578e-4};
    // intrinsic matrix
    Eigen::Matrix3d K;
    K << 4.616e2, 0, 3.63e2,
         0, 4.603e2, 2.481e2,
         0, 0, 1;
    
    Eigen::Vector2d u{40., 50.};
    Eigen::Vector2d u_u, u_d;

    distortion(u, u_d, K, D);
    cout << "Distorted point coordinates are:" << endl << u_d << endl;

    double tolerance(1e-4);
    undistortion(u_d, u_u, K, D, tolerance);
    cout << "Undistorted point coordinates are: " << endl << u_u << endl;

    return 0;
}

void distortion(const Eigen::Vector2d& u_u, Eigen::Vector2d& u_d, 
				const Eigen::Matrix3d& K, const Eigen::Vector4d& D){
    // lift from pixel plane to normalized plane
    Eigen::Vector3d p_u;
    Eigen::Vector3d u_u_homo{0., 0., 1.};
    
    u_u_homo.head(2) = u_u;

    p_u = K.inverse() * u_u_homo;
    // apply distortion
    double x_u, y_u;
    x_u = p_u[0]; y_u = p_u[1];
    
    double k1, k2; // radial distortion coefficients
    double p1, p2; // tangent distortion coefficients
    k1 = D[0]; k2 = D[1];
    p1 = D[2]; p2 = D[3]; 

    double x_u2, y_u2, xy_u, r2;
    x_u2 = x_u * x_u;
    y_u2 = y_u * y_u;
    xy_u = x_u * y_u;
    r2 = x_u2 + y_u2;
    
    Eigen::Vector3d p_d;
    p_d[0] = x_u * (1 + k1 * r2 + k2 * r2 * r2) + (2 * p1 * xy_u + p2 * (r2 + 2 * x_u2));
    p_d[1] = y_u * (1 + k1 * r2 + k2 * r2 * r2) + (2 * p2 * xy_u + p1 * (r2 + 2 * y_u2));
    p_d[2] = 1.;
    // project from normalized plane to normalized plane
    u_d = (K * p_d).head(2);
}

void undistortion(const Eigen::Vector2d& u_d, Eigen::Vector2d& u_u, 
				const Eigen::Matrix3d& K, const Eigen::Vector4d& D, const double tolerance){
    // lift from pixel plane to normalized plane
    Eigen::Vector3d p_d;
    Eigen::Vector3d u_d_homo{0., 0., 1.};

    u_d_homo.head(2)=u_d;
    p_d = K.inverse() * u_d_homo;

    // recursive undistortion
    double k1, k2; // radial distortion coefficients
    double p1, p2; // tangent distortion coefficients
    k1 = D[0]; k2 = D[1];
    p1 = D[2]; p2 = D[3]; 

    double x_d, y_d;
    x_d = p_d[0]; y_d = p_d[1];

    Eigen::Vector3d p_u;
    p_u[2] = 1.0;

    int count(0), conv_count(0);

    while (1){
        
        count++;
        double x_d2, y_d2, xy_d, r2;
        x_d2 = x_d * x_d;
        y_d2 = y_d * y_d;
        xy_d = x_d * y_d;
        r2 = x_d2 + y_d2;

        p_u[0] = (p_d[0] - (2 * p1 * xy_d + p2 * (r2 + 2 * x_d2))) / (1 + k1 * r2 + k2 * r2 * r2);
        p_u[1] = (p_d[1] - (2 * p2 * xy_d + p2 * (r2 + 2 * y_d2))) / (1 + k1 * r2 + k2 * r2 * r2);

        // project from normalized plane to pixel plane
        u_u = (K * p_u).head(2);
        cout << "Iterative times: " << count << "  Estimation: " << u_u.transpose() << endl;
    
        // termination criteria
        double u_err, v_err;
        u_err = abs(K(0, 0) * (p_u[0] - x_d));
        v_err = abs(K(1, 1) * (p_u[1] - y_d));
        if ( u_err < tolerance && v_err < tolerance) conv_count++;           
    
        if (conv_count > 3){
            cout << "Total Iterative times: " << count << endl
                << "Final error: " << u_err << " pixels in x and " << v_err << " pixels in y." << endl
                << "Final estimation: " << u_u.transpose() << endl;
            break;
        }

        x_d = p_u[0]; y_d = p_u[1];

    }
}

鱼眼相机模型

Kannala畸变模型

鱼眼相机的成像原理并不同于普通的真空相机模型，而是一个半球面映射过程，包括透射映射、等距映射、等立体角映射、正交投影、球极投影等。这里仅介绍OpenCV中使用的等距投影模型，Kannala畸变投影模型。

上图给出了鱼眼相机的等距成像模型，所谓等距模型，即空间点在像平面的像高$r$仅与入射角$\theta$有关，且两者之间为等比关系，即$r=f\theta$。

上图进一步给出了鱼眼相机在等距模型下的完整成像关系。假设空间点在经过外参变换后在相机坐标系下的坐标为,$\mathbf{P}=[X^c,Y^c,Z^c{]}^T$，将其缩放至归一化平面有：
$$\begin{array}{rl}{X}_n^c& =X^c/Z^c\\ Y_n^c& =Y^c/Z^c\end{array}\text{\hspace{1em}(1)}$$
则入射角$\theta$可以表示为：
$$\begin{array}{rl}{r}^c& =\sqrt{{X_n^c}^2+{X_n^c}^2}\\ \theta & =atan(r^c/1)=atan(r^c)\end{array}\text{\hspace{1em}(2)}$$
进而根据等距投影模型可以得到像点的像高为：$r^i=f\theta$，利用相似三角形关系，同时考虑成像面为归一化成像面（$f=1$）可得：
$$\{\begin{array}{rl}\frac{X_n^c}{r^c}& =\frac{x}{r^i}\\ \frac{Y_n^c}{r^c}& =\frac{y}{r^i}\end{array}\Rightarrow \{\begin{array}{rl}x& =\frac{f\theta }{r^c}{X}_n^c\\ y& =\frac{f\theta }{r^c}{Y}_n^c\end{array}\Rightarrow \{\begin{array}{rl}x& =\frac{\theta }{r^c}{X}_n^c\\ y& =\frac{\theta }{r^c}{Y}_n^c\end{array}\text{\hspace{1em}(3)}$$
式（1）-（3）即为理想情况下鱼眼相机的等距投影模型，实际成像中，鱼眼相机的畸变会引起入射角的变化，Kannala给出的鱼眼畸变模型如下：
$${\theta }_d=\theta +k_1{\theta }^3+k_2{\theta }^5+k_3{\theta }^7+k_4{\theta }^9\text{\hspace{1em}(4)}$$
将式（4）计算得到的${\theta }_d$带入式（3）中替换$\theta$即可得到存在畸变的鱼眼相机等距投影模型。
在获取像平面坐标$\mathbf{p}=[x,y,z{]}^T$后，基于相机内参矩阵$\mathbf{K}$获得像点在像素平面上的坐标，由于这一过程与针孔相机模型完全一致，这里略过不谈。

鱼眼相机畸变校正

在上述鱼眼相机等距投影模型中，可以记空间点$\mathbf{P}$与鱼眼透镜光心$\mathbf{O}$连线与透镜的交点为${\mathbf{P}}_1=[X_1^c,Y_1^c,Z_1^c{]}^T$，将${\mathbf{P}}_1$表示为极坐标形式有：
$$\begin{array}{rl}{X}_1^c& =R\sin (\theta )\cos (\phi )\\ Y_1^c& =R\sin (\theta )\sin (\phi )\\ Z_1^c& =R\cos (\theta )\end{array}$$
由于通常我们在归一化球面（$R=1$）上进行投影，鱼眼相机畸变校正等效于已知像点坐标$\mathbf{p}=[x,y,z{]}^T$和畸变参数$k_1,k_2,k_3,k_4$，获得无畸变情况下的极坐标参数$\theta$和$\phi$。

• $\phi$求解
  成像面上夹角${\phi }_1$可以用像点坐标表示为：$\tan ({\phi }_1)=y/x$。由于入射光线和出射光线必定在同一个入射面内，有$\phi ={\phi }_1$，因此未畸变的极坐标参数$\phi$为：
  $$\phi =\tan (y/x)$$
• $\theta$求解
  畸变后的入射角${\theta }_d$可以用像点坐标表示为$\theta =\sqrt{(x^2+y^2)}$，$\theta$角可通过牛顿迭代求解式（4）所表示的多项式方程获得，令：
  $$f(\theta )=\theta +k_1{\theta }^3+k_2{\theta }^5+k_3{\theta }^7+k_4{\theta }^9-{\theta }_d$$
  则根据牛顿迭代有：
  $${\theta }_{k+1}={\theta }_k-\frac{f(\theta )}{f^{\prime }(\theta )}$$