相机模型--针孔相机投影（pinhole camera model）

最新推荐文章于 2025-02-05 15:21:35 发布

幻想成为大牛的小不牛

最新推荐文章于 2025-02-05 15:21:35 发布

阅读量4.4k

点赞数 6

分类专栏：相机投影模型

本文链接：https://blog.csdn.net/weixin_43304707/article/details/113128774

版权

相机投影模型专栏收录该内容

4 篇文章

订阅专栏

针孔相机投影（pinhole camera model）

针孔相机模型
畸变模型
去畸变
3D->2D转换
2D->3D转换
雅可比计算

近来感到无比焦躁，可能是临近而立又平平无奇的缘故吧（求大佬们带飞），为了缓解这种焦虑，于是打算对自己工作以来接触到的相关内容进行一个总结，这对于我这种从来不记笔记的人来说，简直是个灾难，不知道这是要缓解焦虑还是加重焦虑，哎算了，不管了，先从最简单的相机模型开始总结。

针孔相机模型

针孔相机的原理就是初中时代学的小孔成像，在这个模型中，会用到3种坐标系：世界坐标系，相机坐标系以及图像像素坐标系。如图所示。
在这里插入图片描述
将针孔相机模型转换成数学模型，具体表示为：
$sp = K{P_c} = K[R|t]{P_w}$
其中, $P_w$ 表示世界坐标系下的坐标， $P_c$ 表示相机坐标系下的坐标， $p$ 表示图像像素坐标系下的坐标， $K$ 表示相机的内参矩阵。将上式写的更具体一些为
在这里插入图片描述

畸变模型

由于光线穿过镜头时会发生折射，因此实际成像位置与理论成像位置存在一些偏差，另外镜头安装误差也会导致成像位置发生变化，我们成这些为畸变。一般与针孔模型搭配使用的畸变模型就是著名的Brown畸变模型，模型包含5个参数，3个径向畸变参数和2个切向畸变参数，该模型的具体形式如下
${x_d} = x(1 + {k_1}{r^2} + {k_2}{r^4} + {k_3}{r^6}) + 2{p_1}xy + {p_2}({r^2} + 2{x^2}) \\ y_d = y(1 + {k_1}{r^2} + {k_2}{r^4} + {k_3}{r^6}) + 2{p_2}xy + {p_1}({r^2} + 2{y^2})$

其中， $p = (x, y, 1)$ 表示相机坐标系中的点归一化到单位焦平面上的坐标， $p_d=(x_d, y_d, 1)$ 表示加入畸变后的坐标。

去畸变

上述公式可以认为是对点进行加畸变的过程，那么怎么去畸变呢，本人比较常用的方法有以下两种，都是通过优化的方法来求解，废话不多说，直接上代码
方法1：

template<typename DERIVED_P>
void RadialTangentialDistortion::undistort(const Eigen::MatrixBase<DERIVED_P> &p_d,
                                           const Eigen::MatrixBase<DERIVED_P> &p_ud) const
        {
            EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(Eigen::MatrixBase<DERIVED_P>, 2)
            Eigen::MatrixBase<DERIVED_P> &y_ud = const_cast<Eigen::MatrixBase<DERIVED_P> &>(p_ud);
            Eigen::MatrixBase<DERIVED_P> &y_d = const_cast<Eigen::MatrixBase<DERIVED_P> &>(p_d);
            y_ud.derived().resize(2);
            y_d.derived().resize(2);

            struct MLFunctor{
                MLFunctor( const Eigen::Vector2d &imagePoint, double k1, double k2, double k3, double p1, double p2)
                        : imagePoint_(imagePoint), _k1(k1), _k2(k2), _k3(k3), _p1(p1), _p2(p2)
                {
                }

                int operator( )(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const{
                    double xx = x[0] * x[0];
                    double yy = x[1] * x[1];
                    double xy = x[0] * x[1];
                    double r2 = xx + yy;
                    double r4 = r2 * r2;
                    double r6 = r4 * r2;

                    double d = 1 + _k1 * r2 + _k2 * r4 + _k3 * r6;
                    fvec[0] = x[0] * d + 2.0 * _p1 * xy + _p2 * (r2 + 2.0 * xx);
                    fvec[1] = x[1] * d + 2.0 * _p2 * xy + _p1 * (r2 + 2.0 * yy);
                    fvec = fvec - imagePoint_;
                    return 0;
                }

                int df( const Eigen::VectorXd &x, Eigen::MatrixXd &fjac ) const
                {
                    double xx = x[0] * x[0];
                    double yy = x[1] * x[1];
                    double xy = x[0] * x[1];
                    double r2 = xx + yy;
                    double r4 = r2 * r2;
                    double r6 = r4 * r2;

                    fjac(0, 0) = 1 + (3 * xx + yy) * _k1 + (4 * xx * r2 + r4) * _k2 + (r6 + 6 * xx * r4) * _k3 + 2 * x[1] * _p1 + 6 * x[0] * _p2;
                    fjac(0, 1) = 2 * xy * _k1 + 4 * xy * r2 * _k2 + 6 * xy * r4 * _k3 + 2 * x[0] * _p1 + 2 * x[1] * _p2;
                    fjac(1, 0) = fjac(0, 1);
                    fjac(1, 1) = 1 + (xx + 3 * yy) * _k1 + (4 * yy * r2 + r4) * _k2 + (r6 + 6 * yy * r4) * _k3 + 6 * x[1] * _p1 + 2 * x[0] * _p2;

                    return 0;
                }

                int values() const { return 2; }
                int inputs() const { return 2; }

                Eigen::Vector2d imagePoint_;

                double _k1;
                double _k2;
                double _k3;
                double _p1;
                double _p2;
            };

            Eigen::Vector2d target = y_d;
            MLFunctor func(target, m_k1, m_k2, m_k3, m_p1, m_p2);
            Eigen::LevenbergMarquardt< MLFunctor > lm(func);
            double x = y_d[0];
            double y = y_d[1];

            Eigen::VectorXd res = Eigen::Vector2d(x, y);
            lm.minimize(res);
            y_ud = res;
        }

方法2：

template <typename T>
void distortPointFun(T x_ud, T y_ud, T *x_d, T *y_d, T k1, T k2, T k3, T p1, T p2){

    T xx = x_ud * x_ud;
    T yy = y_ud * y_ud;
    T xy = x_ud * y_ud;

    T r2 = xx + yy;
    T r4 = r2 * r2;
    T r6 = r4 * r2;

    T d = k1 * r2 + k2 * r4 + k3 * r6;
    *x_d = x_ud * d + T(2.0) * p1 * xy + p2 * (r2 + T(2.0) * xx);
    *y_d = y_ud * d + T(2.0) * p2 * xy + p1 * (r2 + T(2.0) * yy);
}

template <typename T>
void undistortPoint(T x_d, T y_d, T *x_ud, T *y_ud, T k1, T k2, T k3, T p1, T p2){

    T epsilon = T(1e-10);
    *x_ud = x_d;
    *y_ud = y_d;
    T x_tmp, y_tmp;
    distortPointFun(*x_ud, *y_ud, &x_tmp, &y_tmp, k1, k2, k3, p1, p2);
    int n = 0;
    while(n < 20 && sqrt((*x_ud + x_tmp - x_d) * (*x_ud + x_tmp - x_d) + (*y_ud + y_tmp - y_d) * (*y_ud + y_tmp - y_d)) > epsilon){
        *x_ud = x_d - x_tmp;
        *y_ud = y_d - y_tmp;
        distortPointFun(*x_ud, *y_ud, &x_tmp, &y_tmp, k1, k2, k3, p1, p2);
        n++;
    }
    if(n >= 20){
        *x_ud = x_d;
        *y_ud = y_d;
    }
}

3D->2D转换

3D点到2D点的转换步骤如下：
1）对给定的世界坐标系下的一点 $P_w=(X_w, Y_w,Z_w)$ ，应用投影矩阵 $T = [R ∣ t]$ ，得到该点在相机坐标系下的坐标 $P_c=(X_c,Y_c,Z_c)=TP_w$ 。
2）将 $P_c$ 归一化到焦平面上，得到 $P_u=(x, y, 1) = (X_c / Z_c, Y_c/Z_c,1)$ 。
3）应用畸变参数，得到带畸变的点 $P_d=(x_d, y_d, 1)$ 。
4）应用内参得到该点在图像像素坐标系下的坐标 $p=(u, v, 1) = KP_d$

2D->3D转换

2D点到3D点的转换步骤如下：
1）给定图像上一点 $p = (u, v, 1)$ , 先将其转换到归一化焦平面上 $P_d=(x_d,y_d,1)=K^{-1}p$ 。
2）进行去畸变，得到对应的无畸变的点 $P_u=(x,y,1)$ 。
3）如果知道当前点的深度值d，那么就可以将点投影到相机坐标系下 $P_c=dp_u$ ，否则就只能到此。
4）通过投影矩阵，将相机坐标系下的点转换到世界坐标系下 $P_w=R^{-1}(P_c-t)$ 。

雅可比计算

多数情况下，都用BA算法来计算相机的内外参，这就需要知道雅可比矩阵，即投影误差对各待优化变量的偏导数组成的矩阵。所谓的投影误差，实际检测到点与投影到图像上的点之间的误差
$err=p_m - p$
其中 $p_m$ 表示检测到的角点。为了避免手撕公式，我使用了matlab直接来推导出结果，并且在推导公式时，没有考虑畸变项，因为公式太长了，懒得敲。
代码：

syms fx fy x y z cx cy k1 k2 k3 p1 p2

x_u = x / z;
y_u = y / z;
r2 = x_u^2+y_u^2;
r4 = r2^2;
r6 = r2^3;
%x_d = x_u * (1 + k1 * r2 + k2 * r4 + k3 * r6) + 2 * p1 * x_u * y_u + p2 * (r2 + 2 * x_u^2);
%y_d = y_u * (1 + k1 * r2 + k2 * r4 + k3 * r6) + 2 * p2 * x_u * y_u + p1 * (r2 + 2 * y_u^2);
x_d = x_u;
y_d = y_u;
u = fx * x_d + cx;
v = fy * y_d + cy;

alphaE_alphaK = - [diff(u, fx), diff(u, fy), diff(u, cx), diff(u, cy);
                   diff(v, fx), diff(v, fy), diff(v, cx), diff(v, cy)]

alphaE_alphaP = -[diff(u, x), diff(u, y), diff(u, z);
                diff(v, x), diff(v, y), diff(v, z)]
alphaP_alphaR = [1, 0, 0, 0, z, -y; 
                 0, 1, 0, -z, 0, x; 
                 0, 0, 1, y, -x, 0]

alphaE_alphaP * alphaP_alphaR