針孔相機模型

針孔相機的原理就是初中時代學的小孔成像，在這個模型中，會用到3種坐標系：世界坐標系，相機坐標系以及影像像素坐標系，如圖所示，

將針孔相機模型轉換成數學模型，具體表示為：
s p = K P c = K [ R ∣ t ] P w sp = K{P_c} = K[R|t]{P_w} sp=KPc?=K[R∣t]Pw?
其中, P w P_w Pw?表示世界坐標系下的坐標， P c P_c Pc?表示相機坐標系下的坐標， p p p表示影像像素坐標系下的坐標， K K K表示相機的內參矩陣，將上式寫的更具體一些為

畸變模型

由于光線穿過鏡頭時會發生折射，因此實際成像位置與理論成像位置存在一些偏差，另外鏡頭安裝誤差也會導致成像位置發生變化，我們成這些為畸變，一般與針孔模型搭配使用的畸變模型就是著名的Brown畸變模型，模型包含5個引數，3個徑向畸變引數和2個切向畸變引數，該模型的具體形式如下
x 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 d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 2 x y + p 1 ( r 2 + 2 y 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}) xd?=x(1+k1?r2+k2?r4+k3?r6)+2p1?xy+p2?(r2+2x2)yd?=y(1+k1?r2+k2?r4+k3?r6)+2p2?xy+p1?(r2+2y2)

其中， p = ( x , y , 1 ) p=(x, y, 1) p=(x,y,1)表示相機坐標系中的點歸一化到單位焦平面上的坐標， p d = ( x d , y d , 1 ) p_d=(x_d, y_d, 1) pd?=(xd?,yd?,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 ) P_w=(X_w, Y_w,Z_w) Pw?=(Xw?,Yw?,Zw?)，應用投影矩陣 T = [ R ∣ t ] T=[R|t] T=[R∣t]，得到該點在相機坐標系下的坐標 P c = ( X c , Y c , Z c ) = T P w P_c=(X_c,Y_c,Z_c)=TP_w Pc?=(Xc?,Yc?,Zc?)=TPw?，
2）將 P c P_c Pc?歸一化到焦平面上，得到 P u = ( x , y , 1 ) = ( X c / Z c , Y c / Z c , 1 ) P_u=(x, y, 1) = (X_c / Z_c, Y_c/Z_c,1) Pu?=(x,y,1)=(Xc?/Zc?,Yc?/Zc?,1)，
3）應用畸變引數，得到帶畸變的點 P d = ( x d , y d , 1 ) P_d=(x_d, y_d, 1) Pd?=(xd?,yd?,1)，
4）應用內參得到該點在影像像素坐標系下的坐標 p = ( u , v , 1 ) = K P d p=(u, v, 1) = KP_d p=(u,v,1)=KPd?

2D->3D轉換

2D點到3D點的轉換步驟如下：
1）給定影像上一點 p = ( u , v , 1 ) p=(u, v, 1) p=(u,v,1), 先將其轉換到歸一化焦平面上 P d = ( x d , y d , 1 ) = K ? 1 p P_d=(x_d,y_d,1)=K^{-1}p Pd?=(xd?,yd?,1)=K?1p，
2）進行去畸變，得到對應的無畸變的點 P u = ( x , y , 1 ) P_u=(x,y,1) Pu?=(x,y,1)，
3）如果知道當前點的深度值d，那么就可以將點投影到相機坐標系下 P c = d p u P_c=dp_u Pc?=dpu?，否則就只能到此，
4）通過投影矩陣，將相機坐標系下的點轉換到世界坐標系下 P w = R ? 1 ( P c ? t ) P_w=R^{-1}(P_c-t) Pw?=R?1(Pc??t)，

雅可比計算

多數情況下，都用BA演算法來計算相機的內外參，這就需要知道雅可比矩陣，即投影誤差對各待優化變數的偏導陣列成的矩陣，所謂的投影誤差，實際檢測到點與投影到影像上的點之間的誤差
e r r = p m ? p err=p_m - p err=pm??p
其中 p m p_m pm?表示檢測到的角點，為了避免手撕公式，我使用了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 P_c Pc?的偏導
   

3. 誤差項在李代數上的擾動模型
   根據鏈式法則可得

? e r r ? δ ξ = ? e r r ? P c ? P c ? δ ξ {{\partial err} \over {\partial \delta \xi }} = {{\partial err} \over {\partial {P_c}}}{{\partial {P_{\rm{c}}}} \over {\partial \delta \xi }} ?δξ?err?=?Pc??err??δξ?Pc??

其中， ? P c ? δ ξ {{\partial {P_{\rm{c}}}} \over {\partial \delta \xi }} ?δξ?Pc??的推導后續會有專門篇幅進行總結，在這個先用起來再說，