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

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

针孔相机模型

针孔相机的原理就是初中时代学的小孔成像，在这个模型中，会用到3种坐标系：世界坐标系，相机坐标系以及图像像素坐标系。如图所示。

将针孔相机模型转换成数学模型，具体表示为：
$$sp=K{P}_c=K[R|t]P_w$$
其中, $P_w$表示世界坐标系下的坐标，$P_c$表示相机坐标系下的坐标，$p$表示图像像素坐标系下的坐标，$K$表示相机的内参矩阵。将上式写的更具体一些为

畸变模型

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

其中，$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)=T{P}_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)=K{P}_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=d{p}_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

1. 误差项关于内参的偏导数
   

2. 误差项关于相机坐标系下点$P_c$的偏导
   

3. 误差项在李代数上的扰动模型
   根据链式法则可得

$$\frac{\partial err}{\partial \delta \xi }=\frac{\partial err}{\partial P_c}\frac{\partial P_{\mathrm{c}}}{\partial \delta \xi }$$

其中，$\frac{\partial P_{\mathrm{c}}}{\partial \delta \xi }$的推导后续会有专门篇幅进行总结，在这个先用起来再说。