魚眼相機模型 （fisheye camera model）
之前總結了一下針孔相機的模型，然後得到了比較積極的回覆（其實是我到處求人關注的，雖然截至到目前才三個人），所以就再接再勵，乘勝追擊（也沒得辦法，誇下的海口，跪着也要做完），繼續總結其他相機模型。
模型介紹
魚眼相機相較於針孔相機來說，視野更廣，但畸變也更加嚴重，因此普通的針孔模型已不適用。魚眼鏡頭的基本構造如圖所示，經過多個鏡頭的折射，最終到達成像單元上。文章中所有圖片均來自於網絡，非本人所繪。
一般情況下，可以通過控制光線的路徑來設計各種各樣的鏡頭類型，根據投影方式的不同，可以將這些鏡頭分爲等距投影、等立體角投影、正交投影、體視投影以及線性投影。
等距投影
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;
}
投影過程
- 給定相機座標系下一點 P c = ( x , y , z ) P_c=(x, y, z) Pc=(x,y,z)
- 歸一化到球平面上 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)
- 求經緯度 θ = 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)
- 利用畸變模型對 θ \theta θ進行加畸變處理，得到 θ d \theta_d θd。
- 最後得到該點在圖像中的像素座標 u = f x θ d x c r + c x u={ {f_x\theta_d x_c}\over r } +cx u=rfxθdxc+cx, v = f y θ d y c r + c y v={ {f_y\theta_d y_c}\over r } +cy v=rfyθdyc+cy, r = x c 2 + y c 2 r=\sqrt{x_c^2 + y_c^2} r=xc2+yc2
反投影過程
- 給定圖像上一點 p = ( u , v ) p=(u, v) p=(u,v).
- 可以得到歸一化座標 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)=(fxu−cx，fyv−cy)
- 可以得到帶畸變的入射角 θ 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).
- 去畸變後得到 θ \theta θ.
- 最後得到單位球面座標 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)
-
誤差項關於內參的偏導數
![在這裏插入圖片描述]()
其中 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)。 -
誤差項關於相機座標系下點 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)。 -
誤差項在李代數上的擾動模型
根據鏈式法則可得
∂ 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)。