图像雅可比矩阵原理与推导
理想情况下，图像像素坐标系和图像物理坐标系无倾斜，则二者坐标转换关系如下，且两边求导：
[ u v 1 ] = [ 1 d x 0 u 0 0 1 d y v 0 0 0 1 ] [ x y 1 ] (1) \begin{bmatrix}u\\v\\1\end{bmatrix}=\begin{bmatrix}\frac{1}{d_x}&0&u_0\\0&\frac{1}{d_y}&v_0\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} \tag{1} uv1 = dx1000dy10u0v01 xy1 (1) { u ˙ = 1 d x x ˙ v ˙ = 1 d y y ˙ (2) \begin{cases}\dot{u}=\frac{1}{d_x}\dot{x}\\ \dot{v}=\frac{1}{d_y}\dot{y}\end{cases} \tag{2} {u˙=dx1x˙v˙=dy1y˙(2)由小孔成像原理，空间一点的相机坐标和图像物理坐标转换关系如下，且两边求导：
[ x y 1 ] = [ f Z c 0 0 0 f Z c 0 0 0 1 Z c ] [ X c Y c Z c ] (3) \begin{bmatrix}x\\ y\\ 1\end{bmatrix}=\begin{bmatrix}\frac{f}{Z_c}&0&0\\ 0&\frac{f}{Z_c}&0\\ 0&0&\frac{1}{Z_c}\end{bmatrix}\begin{bmatrix}X_c\\ Y_c\\ Z_c\end{bmatrix} \tag{3} xy1 = Zcf000Zcf000Zc1 XcYcZc (3) { x ˙ = f ( X ˙ c Z c − X c Z ˙ c Z c 2 ) = f X ˙ c Z c − x Z ˙ c Z c y ˙ = f ( Y ˙ c Z c − Y c Z ˙ c Z c 2 ) = f Y ˙ c Z c − y Z ˙ c Z c (4) \begin{cases}\dot{x}=f(\frac{\dot{X}_c}{Z_c}-\frac{X_c\dot{Z}_c}{Z_c^2})=\frac{f\dot{X}_c}{Z_c}-\frac{x\dot{Z}_c}{Z_c}\\ \dot{y}=f(\frac{\dot{Y}_c}{Z_c}-\frac{Y_c\dot{Z}_c}{Z_c^2})=\frac{f\dot{Y}_c}{Z_c}-\frac{y\dot{Z}_c}{Z_c}\end{cases} \tag{4} {x˙=f(ZcX˙c−Zc2XcZ˙c)=ZcfX˙c−ZcxZ˙cy˙=f(ZcY˙c−Zc2YcZ˙c)=ZcfY˙c−ZcyZ˙c(4)固定相机，移动空间点时，速度关系为： p ˙ c = c v p + c ω p × p c (5) \dot{\boldsymbol{p}}_c =^c\boldsymbol{v}_p +^c\boldsymbol{\omega}_p\times\boldsymbol{p}_c\tag{5} p˙c=cvp+cωp×pc(5)固定空间点，移动相机时，速度关系为： p ˙ c = − c v c − c ω c × p c (6) \dot{\boldsymbol{p}}_c = -^c\boldsymbol{v}_c -^c\boldsymbol{\omega}_c\times\boldsymbol{p}_c\tag{6} p˙c=−cvc−cωc×pc(6) { X ˙ c = − c ν c , x − c ω c , y Z c + c ω c , z Y c Y ˙ c = − c ν c , y − c ω c , z X c + c ω c , x Z c Z ˙ c = − c ν c , z − c ω c , x Y c + c ω c , y X c (7) \begin{cases}\dot{X}_c=-{}^c\nu_{c,x}-{}^c\omega_{c,y}Z_c+{}^c\omega_{c,z}Y_c\\ \dot{Y}_c=-{}^c\nu_{c,y}-{}^c\omega_{c,z}X_c+{}^c\omega_{c,x}Z_c\\ \dot{Z}_c=-{}^c\nu_{c,z}-{}^c\omega_{c,x}Y_c+{}^c\omega_{c,y}X_c\end{cases}\tag{7} ⎩ ⎨ ⎧X˙c=−cνc,x−cωc,yZc+cωc,zYcY˙c=−cνc,y−cωc,zXc+cωc,xZcZ˙c=−cνc,z−cωc,xYc+cωc,yXc(7)将(7)代入(4)，得： { x ˙ = − f Z c c v c , x + x Z c c v c , z + x y f c ω c , x − f 2 + x 2 f c ω c , y + y c ω c , z y ˙ = − f Z c c v c , y + y Z c c v c , z + f 2 + y 2 f c ω c , x − x y f c ω c , y − x c ω c , z (8) \left\{\begin{array}{l} \dot{x}=-\frac{f}{Z_{c}}{ }^{c} v_{c, x}+\frac{x}{Z_{c}}{ }^{c} v_{c, z}+\frac{x y}{f}{ }^{c} \omega_{c, x}-\frac{f^{2}+x^{2}}{f}{ }^{c} \omega_{c, y}+y^{c} \omega_{c, z} \\ \dot{y}=-\frac{f}{Z_{c}}{ }^{c} v_{c, y}+\frac{y}{Z_{c}}{ }^{c} v_{c, z}+\frac{f^{2}+y^{2}}{f}{ }^{c} \omega_{c, x}-\frac{x y}{f}{ }^{c} \omega_{c, y}-x^{c} \omega_{c, z} \end{array}\right.\tag{8} {x˙=−Zcfcvc,x+Zcxcvc,z+fxycωc,x−ff2+x2cωc,y+ycωc,zy˙=−Zcfcvc,y+Zcycvc,z+ff2+y2cωc,x−fxycωc,y−xcωc,z(8)即： [ x ˙ y ˙ ] = [ − f Z c 0 x Z c x y f − f 2 + x 2 f y 0 − f Z c y Z c f 2 + y 2 f − x y f − x ] [ c v c , x c v c , y c v c , z c ω c , x c ω c , y c ω c , z ] (9) \begin{bmatrix}\dot{x}\\ \dot{y}\end{bmatrix}=\begin{bmatrix}-\frac{f}{Z_c}&0&\frac{x}{Z_c}&\frac{xy}{f}&-\frac{f^2+x^2}{f}&y\\ 0&-\frac{f}{Z_c}&\frac{y}{Z_c}&\frac{f^2+y^2}{f}&-\frac{xy}{f}&-x\end{bmatrix}\left[\begin{array}{l} { }^{c} v_{c, x} \\ { }^{c} v_{c, y} \\ { }^{c} v_{c, z} \\ { }^{c} \omega_{c, x} \\ { }^{c} \omega_{c, y} \\ { }^{c} \omega_{c, z} \end{array}\right]\tag{9} [x˙y˙]=[−Zcf00−ZcfZcxZcyfxyff2+y2−ff2+x2−fxyy−x] cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z (9)将(9)以及 x = d x ( u − u 0 ) x=d_{x}\left(u-u_{0}\right) x=dx(u−u0)和 y = d y ( v − v 0 ) y=d_y(v-v_0) y=dy(v−v0)代入(2)： [ u ˙ v ˙ ] = [ − f d x Z c 0 ( u − u 0 ) Z c ( u − u 0 ) d y ( v − v 0 ) f − f 2 + d x 2 ( u − u 0 ) 2 d x f d y ( v − v 0 ) d x 0 − f d y Z c ( v − v 0 ) Z c f 2 + d y 2 ( v − v 0 ) 2 d y f − d x ( u − u 0 ) ( v − v 0 ) f − d x ( u − u 0 ) d y ] [ c v c , x c v c , y c v c , z c ω c , x c ω c , y c ω c , z ] (10) \left[\begin{array}{c} \dot{u} \\ \dot{v} \end{array}\right]=\left[\begin{array}{cccccc} -\frac{f}{d_{x} Z_{c}} & 0 & \frac{\left(u-u_{0}\right)}{Z_{c}} & \frac{\left(u-u_{0}\right) d_{y}\left(v-v_{0}\right)}{f} & -\frac{f^{2}+d_{x}^{2}\left(u-u_{0}\right)^{2}}{d_{x} f} & \frac{d_{y}\left(v-v_{0}\right)}{d_{x}} \\ 0 & -\frac{f}{d_{y} Z_{c}} & \frac{\left(v-v_{0}\right)}{Z_{c}} & \frac{f^{2}+d_{y}^{2}\left(v-v_{0}\right)^{2}}{d_{y} f} & -\frac{d_{x}\left(u-u_{0}\right)\left(v-v_{0}\right)}{f} & -\frac{d_{x}\left(u-u_{0}\right)}{d_{y}} \end{array}\right]\left[\begin{array}{l} { }^{c} v_{c, x} \\ { }^{c} v_{c, y} \\ { }^{c} v_{c, z} \\ { }^{c} \omega_{c, x} \\ { }^{c} \omega_{c, y} \\ { }^{c} \omega_{c, z} \end{array}\right] \tag{10} [u˙v˙]= −dxZcf00−dyZcfZc(u−u0)Zc(v−v0)f(u−u0)dy(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0) cvc,xcvc,ycvc,zcωc,xcωc,ycωc,z (10)即： [ u ˙ v ˙ ] = J i m g [ c v c c u c ] (11) \begin{bmatrix}\dot{u}\\ \dot{v}\end{bmatrix}=J_{img}\begin{bmatrix}^c\boldsymbol{v}_{c}\\^c \boldsymbol{u}_{c}\end{bmatrix}\tag{11} [u˙v˙]=Jimg[cvccuc](11)可得图像雅可比矩阵： J i m g = [ − f d x Z c 0 ( u − u 0 ) Z c ( u − u 0 ) d y ( v − v 0 ) f − f 2 + d x 2 ( u − u 0 ) 2 d x f d y ( v − v 0 ) d x 0 − f d y Z c ( v − v 0 ) Z c f 2 + d y 2 ( v − v 0 ) 2 d y f − d x ( u − u 0 ) ( v − v 0 ) f − d x ( u − u 0 ) d y ] (12) J_{img}=\left[\begin{array}{cccccc} -\frac{f}{d_{x} Z_{c}} & 0 & \frac{\left(u-u_{0}\right)}{Z_{c}} & \frac{\left(u-u_{0}\right) d_{y}\left(v-v_{0}\right)}{f} & -\frac{f^{2}+d_{x}^{2}\left(u-u_{0}\right)^{2}}{d_{x} f} & \frac{d_{y}\left(v-v_{0}\right)}{d_{x}} \\ 0 & -\frac{f}{d_{y} Z_{c}} & \frac{\left(v-v_{0}\right)}{Z_{c}} & \frac{f^{2}+d_{y}^{2}\left(v-v_{0}\right)^{2}}{d_{y} f} & -\frac{d_{x}\left(u-u_{0}\right)\left(v-v_{0}\right)}{f} & -\frac{d_{x}\left(u-u_{0}\right)}{d_{y}} \end{array}\right]\tag{12} Jimg= −dxZcf00−dyZcfZc(u−u0)Zc(v−v0)f(u−u0)dy(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0) (12)如有不足之处欢迎指出~
