针孔相机模型

欢迎访问我的博客首页。

  针孔相机模型是像素坐标系、归一化平面坐标系、空间坐标系间的坐标变换的理论依据，在 2D-2D 和 PnP 问题中都会用到。

1. 针孔相机模型

  我们都见过照片，照片可以看成真实景象的一个缩小，比如一个宽高都是几厘米的照片可以拍下一栋大楼。照片的产生原理可以用下图简单表示：

  上图中有三个相互平行的平面和一个蜡烛：左边的平面是成像平面，中间的平面是光心平面，右边的平面是与左边平面对称的平面。按照小孔成像原理，像是在成像平面上的倒像。但我们一般都是正着看照片，所以我们可以把小孔成像模型看成红色框内的成像模型。下图是对红色框内成像模型的更详细呈现：

  相机坐标系是图中原点为光心的红色坐标系，刻度是长度单位。像素坐标系是图中原点在成像平面右上角的黄色坐标系，刻度是像素个数。归一化成像平面是图中的绿色坐标系，它与相机坐标系只有一个倍数关系的差别，这个倍数就是相机坐标系的 Z 轴坐标。

2. 相机内参

  假如相机坐标系中的一个点的坐标是 $P=(X,Y,Z)$，它在归一化成像平面上的坐标是 $n=(x,y)$，相机焦距是 $f$，单位都是米。那么它们的比例关系是：

$$\frac{Z}{f}=\frac{X}{x}=\frac{Y}{y}.$$

  于是：

$$x=f\frac{X}{Z},y=f\frac{Y}{Z}.\text{\hspace{1em}(2.1)}$$

  归一化成像平面和像素坐标系都是二维坐标系。因为它们的刻度不同，所以有比例关系；因为原点不同，所以还有一个平移关系。用单位为像素每米的 ${\alpha }_x$ 、${\alpha }_y$ 表示 $x$、$y$ 方向上的比例关系；用单位为像素的 $c_x$、$c_y$ 表示 $x$、$y$ 方向上的平移关系。则像素坐标 $p=(u,v)$ 可以表示为

$$\{\begin{array}{l}u={\alpha }_{x}x+c_x\\ v={\alpha }_{y}y+c_y\end{array}\text{\hspace{1em}(2.2)}$$

  综合公式 (2.1) 和 (2.2) 得：

$$\{\begin{array}{l}u={\alpha }_{x}f\frac{X}{Z}+c_x\triangleq f_x\frac{X}{Z}+c_x\\ \\ v={\alpha }_{y}f\frac{Y}{Z}+c_y\triangleq f_y\frac{Y}{Z}+c_y\end{array}\text{\hspace{1em}(2.3)}$$

  令 $f_x={\alpha }_{x}f$，$f_y={\alpha }_{y}f$，则公式 (2.3) 可以写成矩阵相乘的形式：

$$Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\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]\triangleq KP.\text{\hspace{1em}(2.4)}$$

  矩阵 $K$ 包含了相机的焦距、横竖方向的缩放和平移系数，称为相机的内参矩阵。有了参数矩阵和距离 $Z$，就可以知道像素坐标系、归一化成像平面坐标系和相机坐标系之间的变换关系。

3. 坐标转换

  相机坐标系到齐次像素坐标系的变换如公式 (2.4)，它可以写成

$$(u,v,1{)}^T=KP/Z.$$

我们把它拆分成方程组，得到相机坐标系到像素坐标系的变换

$$\{\begin{array}{rlr}u& =(X{f}_x+Z{c}_x)/Z& =X{f}_x/Z+c_x\\ c& =(Y{f}_y+Z{c}_y)/Z& =Y{f}_y/Z+c_y\end{array}.\text{\hspace{1em}(3.1)}$$

下面的函数 cam2pix 实现了公式 (3.1)。

  根据公式 (2.4) 还可以知道齐次归一化坐标和齐次像素坐标间的变换关系：

$$\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\\ 1\end{array}\right]=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

我们把它拆分成方程组，得到归一化坐标到像素坐标的变换

$$\{\begin{array}{l}u=x{f}_x+c_x\\ v=x{f}_y+c_y\end{array}\text{\hspace{1em}(3.2)}$$

由公式 (3.2) 还可以得到像素坐标到归一化坐标的变换

$$\{\begin{array}{l}x=(u-c_x)/f_x\\ y=(v-c_y)/f_y\end{array}\text{\hspace{1em}(3.3)}$$

下面的函数 pix2nor 实现了公式 (3.3)。

  下面是相机坐标、归一化成像平面坐标、像素坐标间的坐标变换。由于坐标的单位是米和像素，相除时需要较高精度，所以下面使用 double 实例化模板参数，用 float 可能造成精度不够而使结果误差很大。

// 相机坐标系到像素坐标系的变换。
cv::Point2f cam2pix(const cv::Point3f &p, const cv::Mat &K) {
    return cv::Point2f(
        (p.x * K.at<double>(0, 0)) / p.z + K.at<double>(0, 2),
        (p.y * K.at<double>(1, 1)) / p.z + K.at<double>(1, 2));
}

// 像素坐标系到归一化成像平面的变换。
cv::Point2f pix2nor(const cv::Point2f &p, const cv::Mat &K) {
    return cv::Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

// 相机坐标系到归一化成像平面的变换。
cv::Point2f cam2nor(const cv::Point3f &p, const cv::Mat &K) {
    return pix2nor(cam2pix(p, K), K);
}

// 相机坐标系到归一化成像平面的变换。
cv::Point2f cam2nor(const cv::Point3f &p) {
    return cv::Point2f(p.x / p.z, p.y / p.z);
}

4. 畸变

4.1 畸变过程

  透镜形状引起的畸变称为径向畸变，表现为桶形失真或枕形失真。这种失真离透镜中心越远越严重，因此可以用该距离的多项式表示失真过程

$$\{\begin{array}{rl}{x}_{distorted}& =x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\\ y_{distorted}& =y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\end{array}$$

其中，$k_1$，$k_2$，$k_3$ 称为径向畸变参数。

  透镜安装倾斜引起的畸变称为切向畸变，畸变过程为

$$\{\begin{array}{rl}{x}_{distorted}& =x+2p_{1}xy+p2(r^2+2x^2)\\ x_{distorted}& =y+p1(r^2+2y^2)+2p_{2}xy\end{array}$$

其中，$p_1$，$p_2$ 称为切向畸变参数。

  综上所述，畸变过程为

$$\{\begin{array}{rl}{x}_{distorted}& =x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p2(r^2+2x^2)\\ y_{distorted}& =y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+p1(r^2+2y^2)+2p_{2}xy\end{array}\text{\hspace{1em}(4.1)}$$

4.2 去畸变

  公式 (4.1) 是无畸变归一化坐标 $[x,y{]}^T$ 到有畸变归一化坐标 $[x_{distorted},y_{distorted}{]}^T$ 的过程。我们把它写成下面的形式

$$\{\begin{array}{rl}{x}_{distorted}& =x+\Delta x\\ y_{distorted}& =y+\Delta y\end{array}$$

于是去畸变的过程可以描述为

$$\{\begin{array}{rl}x& =x_{distorted}-\Delta x\\ y& =y_{distorted}-\Delta y\end{array}\text{\hspace{1em}(4.2)}$$

  由于径向畸变的多项式只取了 3 项，切向畸变也同理。所以公式 (4.2) 并没有完全去除畸变，只是让畸变的程度减轻：像素点像透镜中心移动。所以可以通过多次迭代实现更精确的去畸变。下面是 vins-mono 去畸变的代码。

void PinholeCamera::liftProjective(const Eigen::Vector2d &p,   // 有畸变的像素坐标。
                                   Eigen::Vector3d &P) const { // 无畸变的归一化坐标。
    double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u;
    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
    // double lambda;
    // 1.有畸变的像素坐标到有畸变的归一化坐标。Lift points to normalised plane
    mx_d = m_inv_K11 * p(0) + m_inv_K13; // (u - cx) / fx。
    my_d = m_inv_K22 * p(1) + m_inv_K23; // (v - cy) / fy。
    // 2.有畸变的归一化坐标到无畸变的归一化坐标。
    if (m_noDistortion) {
        // 无需畸变矫正。
        mx_u = mx_d;
        my_u = my_d;
    } else {
        // 迭代去畸变模型。Recursive distortion model
        int n = 8;
        Eigen::Vector2d d_u;
        distortion(Eigen::Vector2d(mx_d, my_d), d_u);
        // Approximate value
        mx_u = mx_d - d_u(0);
        my_u = my_d - d_u(1);

        for (int i = 1; i < n; ++i) {
            distortion(Eigen::Vector2d(mx_u, my_u), d_u);
            mx_u = mx_d - d_u(0);
            my_u = my_d - d_u(1);
        }
    }
    // Obtain a projective ray
    P << mx_u, my_u, 1.0;
}

void PinholeCamera::distortion(const Eigen::Vector2d &p_u,   // 归一化成像平面上未纠正的点坐标 (x, y)。
                               Eigen::Vector2d &d_u) const { // 归一化成像平面上已纠正的点坐标 = p_u + d_u。
    // 径向畸变(透镜形状引起)参数。
    double k1 = mParameters.k1();
    double k2 = mParameters.k2();
    // 切向畸变(透镜安装引起)参数。
    double p1 = mParameters.p1();
    double p2 = mParameters.p2();

    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = p_u(0) * p_u(0); // x^2。
    my2_u = p_u(1) * p_u(1); // y^2。
    mxy_u = p_u(0) * p_u(1); // xy。
    rho2_u = mx2_u + my2_u;  // r^2 = x^2 + y^2。
    // 注意：rad_dist_u 少加了个 1(见十四讲公式 5.13)。因此去畸变后的坐标不是 d_u 而是 p_u + d_u。
    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; // k1 * r^2 + k2 * r^4。
    double x_c =
        p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u); // x * (k1 * r^2 + k2 * r^4) + 2 * p1 * xy + p2 * (r^2 + 2 * x^2)。
    double y_c =
        p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); // y * (k1 * r^2 + k2 * r^4) + 2 * p2 * xy + p1 * (r^2 + 2 * y^2)。
    d_u << x_c, y_c;
}

5. 视差公式

  双目相机中经常用到视差公式。同一个物方点在两个像素坐标系上的投影坐标的差叫做视差。下图是双目相机的视差模型，双目相机水平放置，$O_1$ 和 $O_2$ 是两个相机的光心，也是相机坐标系的原点，$x$ 轴方向从 $O_1$ 到 $O_2$，$y$ 轴向下，$z$ 轴向前。

  上图中，蓝线代表像素平面，每个平面在 $x$ 方向上的长度是 $c_x$ 像素。由于我们只关系 $x$ 方向上的视差，所以用线代表平面。空间点 $P$ 在左右像素坐标系上的投影 $p_1$、$p_2$ 的像素横坐标是 $u_1$、$u_2$。$p_1$ 和 $p_2$ 间的距离是 $x$，它们到各自相机坐标系 $z$ 轴的距离是 $x_1$ 和 $x_2$。$P$ 在相机坐标系中的 $z$ 坐标是 $Z$，基线长度是 $b$，焦距是 $f$。图中绿色的字母单位是米，蓝色的字母单位是像素。我们求的视差

$$d=u_1-u_2.\text{\hspace{1em}(5.1)}$$

  首先求

$$\{\begin{array}{r}{x}_1=\frac{u_1-c_x}{{\alpha }_x}\\ x_2=\frac{c_x-u_2}{{\alpha }_x}\end{array}.\text{\hspace{1em}(5.2)}$$

然后

$$x=b-x_1-x_2=b-\frac{u_1-u_2}{{\alpha }_x}=b-\frac{d}{{\alpha }_x}.\text{\hspace{1em}(5.3)}$$

  由三角形 $P{p}_1{p}_2$ 与三角形 $P{O}_1{O}_2$ 相似，知

$$\frac{x}{b}=\frac{Z-f}{Z}.\text{\hspace{1em}(5.4)}$$

从而知

$$x=b-\frac{bf}{Z}.\text{\hspace{1em}(5.5)}$$

  由公式 (5.3) 和公式 (5.5) 知

$$d=\frac{b{\alpha }_{x}f}{Z}=\frac{b{f}_x}{Z}.\text{\hspace{1em}(5.6)}$$

  上面我们只考虑了三维点在两相机之间的情况，对于三维点在双目相机左侧和右侧的情况，可以同理推出公式 (5.6)。

6. 参考

1. Vins 前端中高效的去畸变的方式解析，博客园，2023。