模型介紹

魚眼相機相較于針孔相機來說，視野更廣，但畸變也更加嚴重，因此普通的針孔模型已不適用，魚眼鏡頭的基本構造如圖所示，經過多個鏡頭的折射，最終到達成像單元上，文章中所有圖片均來自于網路，非本人所繪，

一般情況下，可以通過控制光線的路徑來設計各種各樣的鏡頭型別，根據投影方式的不同，可以將這些鏡頭分為等距投影、等立體角投影、正交投影、體視投影以及線性投影，

等距投影

r = f θ r=f \theta r=fθ

等立體角投影

r = 2 f s i n ( θ 2 ) r=2fsin({{\theta} \over {2}}) r=2fsin(2θ?)

正交投影

r = f s i n ( θ ) r=fsin(\theta ) r=fsin(θ)

體視投影

r = 2 f t a n ( θ 2 ) r=2ftan({{\theta} \over {2}}) r=2ftan(2θ?)

線性投影

r = f t a n ( θ ) r=ftan(\theta) r=ftan(θ)

Kannala-Brandt 模型

同針孔模型一樣，魚眼模型也有畸變，那么怎么來用數學魚眼描述這種畸變呢？觀察各種投影方式，發現他們都是一個關于入射角 θ \theta θ 的奇函式，因此魚眼鏡頭的畸變也是對 θ \theta θ 的畸變，KB模型用只包含奇次項的多項式來描述畸變程序，其具體形式如下所示
θ d = θ + k 1 θ 3 + k 2 θ 5 + k 3 θ 7 + k 4 θ 9 \theta_d=\theta+k_1\theta^3+k_2\theta^5+k_3\theta^7+k_4\theta^9 θd?=θ+k1?θ3+k2?θ5+k3?θ7+k4?θ9
opencv中使用的魚眼相機畸變模型就是這個模型，該模型適用于大多數魚眼相機，另外，也可以用這種模型來對普通的針孔相機模型進行標定，仔細觀察，上述的線性投影實際就是普通的針孔投影模型，最后魚眼相機的投影模型可以表示為
r = f θ d r = f \theta_d r=fθd?
其中 r r r表示影像中像素點到主點的距離，

去畸變程序

對于魚眼相機的去畸變程序可以采用決議的方式，因為從上述畸變公式中可以看到，畸變模型是一個多項式，理論上是存在決議解的，但本人使用的方法和針孔模型中類似，通過優化的方法進行去即便，代碼如下

      template<typename DERIVED_P>
      void 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 k4 )
                      : imagePoint_(imagePoint), _k1(k1), _k2(k2), _k3(k3), _k4(k4)
              {
              }

              int operator( )( const Eigen::VectorXd &x, Eigen::VectorXd &fvec ) const{

                  double theta = x[0];
                  double phi = x[1];
                  double theta2 = theta * theta;
				  double theta4 = theta2 * theta2;
				  double theta6 = theta4 * theta2;
				  double theta8 = theta4 * theta4;
                  double thetad = theta * ( 1 + _k1 * theta2 + _k2 * theta4 + _k3 * theta6 + _k4 * theta8);
                  fvec[0] = thetad;
                  fvec[1] = phi;
                  fvec = fvec - imagePoint_;
                  return 0;
              }

              int df( const Eigen::VectorXd &x, Eigen::MatrixXd &fjac ) const
              {
                  double theta = x[0];
                  double theta2 = theta * theta;
				  double theta4 = theta2 * theta2;
				  double theta6 = theta4 * theta2;
				  double theta8 = theta4 * theta4;

                  fjac(0, 0) = 9 * _k4 * theta8 + 7 * _k3 * theta6 + 5 * _k2 * theta4 + 3 * _k1 * theta2 + 1;
                  fjac(0, 1) = 0;
                  fjac(1, 0) = 0;
                  fjac(1, 1) = 1;

                  return 0;
              }

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

              Eigen::Vector2d imagePoint_;

              double _k1;
              double _k2;
              double _k3;
              double _k4;
          };

          Eigen::Vector2d target = y_d;
          MLFunctor func( target, m_k1, m_k2, m_k3, m_k4 );
          Eigen::LevenbergMarquardt< MLFunctor > lm(func);
          double theta = y_d[0];
          double phi = y_d[1];

          Eigen::VectorXd res = Eigen::Vector2d(theta, phi);
          lm.minimize(res);
          y_ud = res;
      }

投影程序

1. 給定相機坐標系下一點 P c = ( x , y , z ) P_c=(x, y, z) Pc?=(x,y,z)
2. 歸一化到球平面上 P s = P c ∣ ∣ P c ∣ ∣ = ( x s , y s , z s ) P_s = {{P_c} \over {||P_c||}}=(x_s, y_s,z_s) Ps?=∣∣Pc?∣∣Pc??=(xs?,ys?,zs?)
3. 求經緯度 θ = a c o s ( z s ) = a t a n 2 ( x 2 + y 2 ) , z ) \theta=acos(z_s)=atan2(\sqrt{x^2+y^2)}, z) θ=acos(zs?)=atan2(x2+y2) ?,z), ? = a t a n 2 ( y s , x s ) \phi=atan2(y_s, x_s) ?=atan2(ys?,xs?)
4. 利用畸變模型對 θ \theta θ進行加畸變處理，得到 θ d \theta_d θd?，
5. 最后得到該點在影像中的像素坐標 u = f x θ d x c r + c x u={{f_x\theta_d x_c}\over r } +cx u=rfx?θd?xc??+cx, v = f y θ d y c r + c y v={{f_y\theta_d y_c}\over r } +cy v=rfy?θd?yc??+cy, r = x c 2 + y c 2 r=\sqrt{x_c^2 + y_c^2} r=xc2?+yc2? ?

反投影程序

1. 給定影像上一點 p = ( u , v ) p=(u, v) p=(u,v).
2. 可以得到歸一化坐標 p u d = ( x , y ) = ( u ? c x f x ， v ? c y f y ) p_{ud} = (x, y) = ({{u - cx} \over f_x}， {{v - cy} \over f_y}) pud?=(x,y)=(fx?u?cx?，fy?v?cy?)
3. 可以得到帶畸變的入射角 θ d = x 2 + y 2 , ? = a t a n 2 ( y , x ) \theta_d=\sqrt{x^2+y^2}, \phi=atan2(y, x) θd?=x2+y2 ?,?=atan2(y,x).
4. 去畸變后得到 θ \theta θ.
5. 最后得到單位球面坐標 x = s i n ( θ ) c o s ( ? ) , y = s i n ( θ ) s i n ( ? ) , z = c o s ( θ ) x = sin(\theta)cos(\phi), y=sin(\theta)sin(\phi), z = cos(\theta) x=sin(θ)cos(?),y=sin(θ)sin(?),z=cos(θ)

雅可比計算

多數情況下，都用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 k4

R = sqrt(x^2 + y^2 + z^2);
xc = x / R;
yc = y / R;
zc = z / R;
theta = acos(z);
%thetad = theta + k1 * theta^3 + k2 * theta^5 + k3 * theta^7 + k4 * theta^9;
thetad = theta;
r = sqrt(xc^2 + yc^2);
u = fx * (thetad * xc) / r + cx;
v = fy * (thetad * yc) / r + 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

假設相機坐標系下歸一化到球面的3D點坐標坐標為 P s = ( x s , y s , z s ) P_s=(x_s, y_s, z_s) Ps?=(xs?,ys?,zs?)

1. 誤差項關于內參的偏導數
   
   其中 r = x s 2 + y s 2 r=\sqrt{x_s^2 + y_s^2} r=xs2?+ys2? ?, θ = a c o s ( z s ) \theta=acos(z_s) θ=acos(zs?)，

2. 誤差項關于相機坐標系下點 P s P_s Ps?的偏導
   
   其中 r = x s 2 + y s 2 r=\sqrt{x_s^2 + y_s^2} r=xs2?+ys2? ?, θ = a c o s ( z s ) \theta=acos(z_s) θ=acos(zs?)，

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

? e r r ? δ ξ = ? e r r ? P s ? P s ? δ ξ {{\partial err} \over {\partial \delta \xi }} = {{\partial err} \over {\partial {P_s}}}{{\partial {P_{\rm{s}}}} \over {\partial \delta \xi }} ?δξ?err?=?Ps??err??δξ?Ps??

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

其中 r = x s 2 + y s 2 r=\sqrt{x_s^2 + y_s^2} r=xs2?+ys2? ?, θ = a c o s ( z s ) \theta=acos(z_s) θ=acos(zs?)，