关于Opencv中棋盘格标定对内外参求导详解

内外参求导详解

参考资料

1、Opencv源码 此cvProjectPoints2()函数

算法原理介绍

1. 成像模型

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$=$\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 1\end{array}\right]$ $(1+k1\ast y^2+k2\ast r^4)$$\left[\begin{array}{cccc}r1& r2& r3& t1\\ r4& r5& r6& t2\\ r7& r8& r9& t3\\ 0& 0& 0& 1\end{array}\right]$$\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$ $\frac{1}{Z}$

此模型为传统模型，不做过多赘述

对此我们将此模型转化为：
$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$=$\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 1\end{array}\right]$$\left[\begin{array}{c}\check{x}\\ \check{y}\\ 1\end{array}\right]$

其中：
$$\{\begin{array}{c}fx\ast \check{x}+cx=u\\ fy\ast \check{y}+cy=v\end{array}$$

$$\{\begin{array}{c}{X}^{\prime }=\frac{r1\ast X+r2\ast Y+r3\ast Z+t1}{r7\ast X+r8\ast Y+r9\ast Z+t3}\\ Y^{\prime }=\frac{r4\ast X+r5\ast Y+r6\ast Z+t2}{r7\ast X+r8\ast Y+r9\ast Z+t3}\end{array}$$

$$\{\begin{array}{c}\check{x}=(1+k1\ast r^2+k2\ast r^4)\ast X^{\prime }\\ \check{y}=(1+k1\ast r^2+k2\ast r^4)\ast Y^{\prime }\end{array}$$

        double X = M[i].x, Y = M[i].y, Z = M[i].z;//世界坐标系
        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
        double xd, yd, xd0, yd0, invProj;
        Vec3d vecTilt;
        Vec3d dVecTilt;
        Matx22d dMatTilt;
        Vec2d dXdYd;

        z = z ? 1./z : 1;
        x *= z; y *= z;//图像坐标系 乘了1/z

        r2 = x*x + y*y;
        r4 = r2*r2;
        r6 = r4*r2;
        a1 = 2*x*y;
        a2 = r2 + 2*x*x;
        a3 = r2 + 2*y*y;
        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;

        // additional distortion by projecting onto a tilt plane
        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
        xd = invProj * vecTilt(0);
        yd = invProj * vecTilt(1);

        m[i].x = xd*fx + cx;
        m[i].y = yd*fy + cy;

首先对fx fy cx cy求导

$${\frac{\mathrm{d}p}{\mathrm{d}fx}|}_x=\check{x}$$
$${\frac{\mathrm{d}p}{\mathrm{d}fy}|}_y=\check{y}$$
$${\frac{\mathrm{d}p}{\mathrm{d}cx}|}_x=1$$
$${\frac{\mathrm{d}p}{\mathrm{d}cy}|}_y=1$$

   if( dpdc_p )
            {
                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
                dpdc_p[dpdc_step] = 0;
                dpdc_p[dpdc_step+1] = 1;
                dpdc_p += dpdc_step*2;
            }
  if( dpdf_p )
            {
                if( fixedAspectRatio )
                {
                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
                    dpdf_p[dpdf_step] = 0;
                    dpdf_p[dpdf_step+1] = yd;
                }
                else
                {
                    dpdf_p[0] = xd; dpdf_p[1] = 0;
                    dpdf_p[dpdf_step] = 0;
                    dpdf_p[dpdf_step+1] = yd;
                }
                dpdf_p += dpdf_step*2;
            }

因为有两个方程都要对fx,fy,cx,cy求导，比如第一个方程关于fx cx的方程与fy cy无关，所以为0

对畸变系数k1,k2 求导 （我们仅仅考虑k1 k2 其他不多叙述）

$${\frac{\mathrm{d}p}{\mathrm{d}k1}|}_x=fx\ast \frac{\mathrm{d}\check{x}}{\mathrm{d}k1}|=fx\ast r^2\ast X^{\prime }$$
$${\frac{\mathrm{d}p}{\mathrm{d}k1}|}_y=fy\ast \frac{\mathrm{d}\check{x}}{\mathrm{d}k1}|=fy\ast r^2\ast Y^{\prime }$$

$${\frac{\mathrm{d}p}{\mathrm{d}k2}|}_x=fx\ast \frac{\mathrm{d}\check{x}}{\mathrm{d}k1}|=fx\ast r^4\ast X^{\prime }$$
$${\frac{\mathrm{d}p}{\mathrm{d}k2}|}_y=fy\ast \frac{\mathrm{d}\check{x}}{\mathrm{d}k1}|=fy\ast r^4\ast Y^{\prime }$$

   if( dpdk_p )
   {
     dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
     dpdk_p[0] = fx*dXdYd(0);
     dpdk_p[dpdk_step] = fy*dXdYd(1);
     dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
     dpdk_p[1] = fx*dXdYd(0);
     dpdk_p[dpdk_step+1] = fy*dXdYd(1);              
     dpdk_p += dpdk_step*2;
   }

下面是关键的对于外参的平移部分T，和旋转部分的R求导
根据此公式
$$\{\begin{array}{c}{X}^{\prime }=\frac{r1\ast X+r2\ast Y+r3\ast Z+t1}{r7\ast X+r8\ast Y+r9\ast Z+t3}=\frac{X^{\prime \prime }}{Z^{\prime \prime }}\\ Y^{\prime }=\frac{r4\ast X+r5\ast Y+r6\ast Z+t2}{r7\ast X+r8\ast Y+r9\ast Z+t3}=\frac{Y^{\prime \prime }}{Z^{\prime \prime }}\end{array}$$

$${\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}t}|}_{t1}=\frac{1}{r7\ast X+r8\ast Y+r9\ast Z+t3}=\frac{1}{Z^{\prime \prime }}$$
$${\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}t}|}_{t2}=0$$
$${\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}t}|}_{t3}=\frac{X^{\prime \prime }}{-Z^{\prime \prime 2}}$$

$${\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}t}|}_{t1}=0$$
$${\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}t}|}_{t2}=\frac{1}{r7\ast X+r8\ast Y+r9\ast Z+t3}=\frac{1}{Z^{\prime \prime }}$$
$${\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}t}|}_{t3}=\frac{Y^{\prime \prime }}{-Z^{\prime \prime 2}}$$

对应opencv中

   double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };

此处在opencv中x,y,z 已经归一化过在代码块1可以看出 $$z=\frac{1}{Z^{\prime \prime }}$$ $$x=\frac{X^{\prime \prime }}{Z^{\prime \prime }}$$ $$y=\frac{Y^{\prime }}{Z^{\prime }}$$

接着我们 r2 求导
$$r^2=X^{\prime 2}+Y^{\prime 2}$$
$${\frac{\mathrm{d}{r}^2}{\mathrm{d}t}|}_{}=2\ast X^{\prime }\ast {\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}t}|}_{}+2\ast Y^{\prime }\ast {\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}t}|}_{}$$

接着我们 畸变函数 求导
$$km=1+k1\ast r^2+k2\ast r^4$$

$${\frac{\mathrm{d}km}{\mathrm{d}t}|}_{}=k1\ast {\frac{\mathrm{d}{r}^2}{\mathrm{d}t}|}_{}+2\ast k2\ast r^2{\frac{\mathrm{d}{r}^2}{\mathrm{d}t}|}_{}$$

$${\frac{\mathrm{d}p}{\mathrm{d}t}|}_x=fx\ast \frac{\mathrm{d}\check{x}}{\mathrm{d}t}|=fx\ast (\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}t}|\ast km+\frac{\mathrm{d}k{m}^{\prime }}{\mathrm{d}t}|\ast X^{\prime })$$
$${\frac{\mathrm{d}p}{\mathrm{d}t}|}_y=fy\ast \frac{\mathrm{d}\check{y}}{\mathrm{d}t}|=fy\ast (\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}t}|\ast km+\frac{\mathrm{d}k{m}^{\prime }}{\mathrm{d}t}|\ast Y^{\prime })$$

    if( dpdt_p )
    {
       double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
       for( j = 0; j < 3; j++ )
        {
           double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
           double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
           double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
           double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
           double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
                            k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
           double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
                                       k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
                    dpdt_p[j] = fx*dXdYd(0);
                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
          }
                dpdt_p += dpdt_step*2;
    }

opencv中利用3次循环对t1 t2 t3 同时求导

对旋转部分的求导比较复杂 首先利用罗格里格斯公式3*3矩阵中每一个元素对rx ry rz
求导可以发现最后的导数为3 * 27的矩阵

            double c = cos(theta);
            double s = sin(theta);
            double c1 = 1. - c;
            double itheta = theta ? 1./theta : 0.;

            r *= itheta;

            Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );
            Matx33d r_x(    0, -r.z,  r.y,
                          r.z,    0, -r.x,
                         -r.y,  r.x,    0 );

            // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
            Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x;

            Mat(R).convertTo(cvarrToMat(dst), dst->type);

            if( jacobian )
            {
                const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
                double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,
                                  0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0,
                                  0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z };
                double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
                                    0, 0, 1, 0, 0, 0, -1, 0, 0,
                                    0, -1, 0, 1, 0, 0, 0, 0, 0 };
                for( i = 0; i < 3; i++ )
                {
                    double ri = i == 0 ? r.x : i == 1 ? r.y : r.z;
                    double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
                    double a3 = (c - s*itheta)*ri, a4 = s*itheta;
                    for( k = 0; k < 9; k++ )
                        J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +
                                   a3*r_x.val[k] + a4*d_r_x_[i*9+k];
                }
            }

$$R=cos(theta)\ast I+(1-cos(theta))\ast r\ast rT+sin(theta)\ast [r_x]$$
$$theta=\sqrt{x^2+y^2+z^2}$$

$$\frac{\mathrm{d}R}{\mathrm{d}x}|=sin(theta)\ast \frac{x}{theta}-sin(theta)\ast \frac{x}{theta}\ast r\ast rT+(1-cos(theta))\ast \frac{\mathrm{d}rrt}{\mathrm{d}x}|+cos(theta)\ast \frac{x}{theta}\ast [r_x]+sin(theta)\ast \frac{\mathrm{d}[r_x]}{\mathrm{d}x}|$$
该式子跟opencv里面的代码对不上，还在找问题中，下一版在更新到底是哪里的问题

               double dx0dr[] =
                {
                    X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
                    X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
                    X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
                };
                double dy0dr[] =
                {
                    X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
                    X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
                    X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
                };
                double dz0dr[] =
                {
                    X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
                    X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
                    X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
                };
                for( j = 0; j < 3; j++ )
                {
                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
                    double dr2dr = 2*x*dxdr + 2*y*dydr;
                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
                    double da1dr = 2*(x*dydr + y*dxdr);
                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
                                       k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
                                       k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
                    dpdr_p[j] = fx*dXdYd(0);
                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
                }
                dpdr_p += dpdr_step*2;

对于$$\frac{\mathrm{d}R}{\mathrm{d}r}|$$ 这个结果排列顺序为
$$\frac{\mathrm{d}r1}{\mathrm{d}x}|,\frac{\mathrm{d}r2}{\mathrm{d}x}|,\frac{\mathrm{d}r3}{\mathrm{d}x}|.........$$
$$\frac{\mathrm{d}r1}{\mathrm{d}y}|,\frac{\mathrm{d}r2}{\mathrm{d}y}|,\frac{\mathrm{d}r3}{\mathrm{d}y}|.........$$
$$\frac{\mathrm{d}r1}{\mathrm{d}z}|,\frac{\mathrm{d}r2}{\mathrm{d}z}|,\frac{\mathrm{d}r3}{\mathrm{d}z}|.........$$
就有式子：

$$\{\begin{array}{c}X\ast =r1\ast X+r2\ast Y+r3\ast Z+t1\\ Y\ast =r4\ast X+r5\ast Y+r6\ast Z+t2\\ Z\ast =r7\ast X+r8\ast Y+r9\ast Z+t3\end{array}$$

$$\frac{\mathrm{d}X\ast }{\mathrm{d}r}|=X\ast \frac{\mathrm{d}r1}{\mathrm{d}x}|+Y\ast \frac{\mathrm{d}r2}{\mathrm{d}x}|+Z\ast \frac{\mathrm{d}r3}{\mathrm{d}x}|=X\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[0]+Y\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[1]+Z\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[2]$$

$$\frac{\mathrm{d}X\ast }{\mathrm{d}r}|=X\ast \frac{\mathrm{d}r1}{\mathrm{d}y}|+Y\ast \frac{\mathrm{d}r2}{\mathrm{d}y}|+Z\ast \frac{\mathrm{d}r3}{\mathrm{d}y}|=X\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[0]+Y\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[1]+Z\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[2]$$

$$\frac{\mathrm{d}X\ast }{\mathrm{d}r}|=X\ast \frac{\mathrm{d}r1}{\mathrm{d}z}|+Y\ast \frac{\mathrm{d}r2}{\mathrm{d}z}|+Z\ast \frac{\mathrm{d}r3}{\mathrm{d}z}|=X\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[0]+Y\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[1]+Z\ast \frac{\mathrm{d}R}{\mathrm{d}r}|[2]$$
这里的xyz其实代表对rx ry rz角度求导

就有了代码里的

               double dx0dr[] =
                {
                    X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
                    X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
                    X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
                }

后面两个同理
因为 X’=X* /Z*
因此 $$\frac{\mathrm{d}{X}^{\prime }}{\mathrm{d}r}|=\frac{1}{Z\ast }\frac{\mathrm{d}X\ast }{\mathrm{d}r}|-\frac{X\ast }{Z{\ast }^2}\frac{\mathrm{d}Z\ast }{\mathrm{d}r}|$$

$$\frac{\mathrm{d}{Y}^{\prime }}{\mathrm{d}r}|=\frac{1}{Z\ast }\frac{\mathrm{d}Y\ast }{\mathrm{d}r}|-\frac{Y\ast }{Z{\ast }^2}\frac{\mathrm{d}Z\ast }{\mathrm{d}r}|$$

     double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
     double dydr = z*(dy0dr[j] - y*dz0dr[j]);

opencv代码里面$$z=\frac{1}{Z\ast }$$ $$x=\frac{X\ast }{Z\ast }$$

    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
                                       k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
                                       k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
    dpdr_p[j] = fx*dXdYd(0);
    dpdr_p[dpdr_step+j] = fy*dXdYd(1);

剩下的部分跟平移部分的求导一致，不在过多叙述

就此我们的成像模型，对fx,fy,cx,cy,k1,k2,R,T,求导都已完成，后续可以形成雅可比矩阵，带入LM非线性方程进行迭代‘

当然还有另一种方法直接放入Ceres中优化。利用Cerres中的自动求导机制，可以避免求导的过程理解