相机模型-鱼眼模型（fisheye camera model）

之前总结了一下针孔相机的模型，然后得到了比较积极的回复（其实是我到处求人关注的，虽然截至到目前才三个人），所以就再接再励，乘胜追击（也没得办法，夸下的海口，跪着也要做完），继续总结其他相机模型。

模型介绍

鱼眼相机相较于针孔相机来说，视野更广，但畸变也更加严重，因此普通的针孔模型已不适用。鱼眼镜头的基本构造如图所示，经过多个镜头的折射，最终到达成像单元上。文章中所有图片均来自于网络，非本人所绘。

一般情况下，可以通过控制光线的路径来设计各种各样的镜头类型，根据投影方式的不同，可以将这些镜头分为等距投影、等立体角投影、正交投影、体视投影以及线性投影。

等距投影

$$r=f\theta$$

等立体角投影

$$r=2fsin(\frac{\theta }{2})$$

正交投影

$$r=fsin(\theta )$$

体视投影

$$r=2ftan(\frac{\theta }{2})$$

线性投影

$$r=ftan(\theta )$$

Kannala-Brandt 模型

同针孔模型一样，鱼眼模型也有畸变，那么怎么来用数学鱼眼描述这种畸变呢？观察各种投影方式，发现他们都是一个关于入射角 $\theta$ 的奇函数，因此鱼眼镜头的畸变也是对 $\theta$ 的畸变，KB模型用只包含奇次项的多项式来描述畸变过程，其具体形式如下所示
$${\theta }_d=\theta +k_1{\theta }^3+k_2{\theta }^5+k_3{\theta }^7+k_4{\theta }^9$$
opencv中使用的鱼眼相机畸变模型就是这个模型，该模型适用于大多数鱼眼相机。另外，也可以用这种模型来对普通的针孔相机模型进行标定，仔细观察，上述的线性投影实际就是普通的针孔投影模型。最后鱼眼相机的投影模型可以表示为
$$r=f{\theta }_d$$
其中$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)$
2. 归一化到球平面上$P_s=\frac{P_c}{||P_c||}=(x_s,y_s,z_s)$
3. 求经纬度 $\theta =acos(z_s)=atan2(\sqrt{x^2+y^2)},z)$, $\varphi =atan2(y_s,x_s)$
4. 利用畸变模型对 $\theta$进行加畸变处理，得到${\theta }_d$。
5. 最后得到该点在图像中的像素坐标$u=\frac{f_x{\theta }_d{x}_c}{r}+cx$, $v=\frac{f_y{\theta }_d{y}_c}{r}+cy$, $r=\sqrt{x_c^2+y_c^2}$

反投影过程

1. 给定图像上一点$p=(u,v)$.
2. 可以得到归一化坐标$p_d=(x,y)=(\frac{u-cx}{f_x}，\frac{v-cy}{f_y})$
3. 可以得到带畸变的入射角 ${\theta }_d=\sqrt{x^2+y^2},\varphi =atan2(y,x)$.
4. 去畸变后得到$\theta$.
5. 最后得到单位球面坐标$x=sin(\theta )cos(\varphi ),y=sin(\theta )sin(\varphi ),z=cos(\theta )$

雅可比计算

多数情况下，都用BA算法来计算相机的内外参，这就需要知道雅可比矩阵，即投影误差对各待优化变量的偏导数组成的矩阵。所谓的投影误差，实际检测到点与投影到图像上的点之间的误差
$$err=p_m-p$$
其中 $p_m$表示检测到的角点。为了避免手撕公式，我使用了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)$

1. 误差项关于内参的偏导数
   
   其中$r=\sqrt{x_s^2+y_s^2}$, $\theta =acos(z_s)$。

2. 误差项关于相机坐标系下点$P_s$的偏导
   
   其中$r=\sqrt{x_s^2+y_s^2}$, $\theta =acos(z_s)$。

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

$$\frac{\partial err}{\partial \delta \xi }=\frac{\partial err}{\partial P_s}\frac{\partial P_{\mathrm{s}}}{\partial \delta \xi }$$

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

其中$r=\sqrt{x_s^2+y_s^2}$, $\theta =acos(z_s)$。