迭代法求模型的参数

迭代优化：

牛顿-- 拉夫逊方法 (Newton-Raphson method)解非线性方程：

问题描述：

为了简单起见使用一个自变量的模型来说明，
假设我们有一个函数模型$y=f(x)=k^2*x^3$y=f(x)=k2∗x3,其中$f(x)$f(x)中有一些未知的参数$k$k未知。而我们已知对应的$x_0,y_0$x0​,y0​, 怎么计算出未知的$k$k呢？不准用解析法。

问题转化：因为这里的$x$x 是已知的$x_0$x0​, $k$k才是未知的。所以令$g(k)=x_0^3k^2 -y_0$g(k)=x03​k2−y0​, 那么$k=?$k=? 能使得$g(k)=x_0^3*k^2 -y_0=0$g(k)=x03​∗k2−y0​=0呢？==>到此可以看出问题已经转化为$g(k)=0$g(k)=0, 方程求解问题了。因此可以利用牛顿法解非线性方程。步骤如下：

• S1:先给$k$k一个任意的初始值$k_0$k0​，正向计算$g(k_0)=x_0^3k_0^2-y_0$g(k0​)=x03​k02​−y0​,注意：这里的$g(k_0)=f(x_0)-y_0$g(k0​)=f(x0​)−y0​,就是原函数的$\Delta y$Δy
• S2:计算$g^{'}(k)=2x_0^3k_0$g′(k)=2x03​k0​在$k_0$k0​处的导数。
• S3:计算$\Delta k={g(k_0) \over g^{'}(k_0)}$Δk=g′(k0​)g(k0​)​.
• 更新$k=k_0 -\Delta k$k=k0​−Δk
• 重复S1-S3 直到$\Delta k$Δk 小于一个很小很小的值

python 代码测试：

import os
# support our function is y=k^2*x**3, k=10,we  give a x0 and y0, please according x0,y0, fit our  function.

#k=10
x0=2
y0=800


def func(k,x):
    y=k**2*x**3
    return y

#df =2k*x**3
def derivative(k,x):
    y=2*k*x**3
    return y

#initial 
k=99
# Optimization 
for intr  in range(0,300):
    y = func(k,x0)    
    delta_y = y-y0   #S1
    df = derivative(k,x0) #s2
    
    #print(y,df,delta_y)
    delta_k = delta_y/df  
    print("delta_k",delta_k)
    k = k - delta_k   #S3
    
    print("k=",k,intr)
    if abs(delta_k) < 0.0001:
        break

执行的结果如下：
delta_k 48.994949494949495
k= 50.005050505050505 0
delta_k 24.002626252424253
k= 26.002424252626252 1
delta_k 11.078314495145142
k= 14.92410975748111 2
delta_k 4.1117712898024505
k= 10.81233846767866 3
delta_k 0.7818226922040425
k= 10.030515775474619 4
delta_k 0.030469356498084566
k= 10.000046418976535 5
delta_k 4.6418868798972104e-05
k= 10.000000000107736 6

扩展到多自变量，多因变量，理论分析如下：

牛顿迭代法可以推广到多元非线性方程组 F(x)=0的情况，称为牛顿-- 拉夫逊方法 (Newton-Raphson method). 当 $F(x)$F(x) 关于 $x$x 的 Jacobi 矩阵$\boldsymbol{J}(\boldsymbol{x}) = (\cfrac{\partial \boldsymbol{F}}{\partial \boldsymbol{x}})$J(x)=(∂x∂F​) 可逆时, 有
$\boldsymbol{x}^{(k+1)}=\boldsymbol{x}^{(k)}- \boldsymbol{J}^{-1}(\boldsymbol{x}^{(k)})\boldsymbol{F}(\boldsymbol{x}^{(k)}),$x(k+1)=x(k)−J−1(x(k))F(x(k)),,

求解步骤：
求解非线性方程组的Newton-Raphson方法：
1、 取初始点 x(0),最大迭代次数 N 和精度要求 ε, 置 k=0;
2、 求解线性方程组 $J(x^k)d=−F(x^k)$J(xk)d=−F(xk);
3、 若 |d|<ε, 则停止计算；否则，置$x^{k+1}=x^k+d^k$xk+1=xk+dk;
4、 若 k=N, 则停止计算；否则，置 k=k+1, 转(2).

联系到这里Calibration具体问题域分析：

假设有15 个自变量（$k_0,k_1,\cdots,k_{14}$k0​,k1​,⋯,k14​），2 个因变量($u,v$u,v)
模型是很复杂的非线性模型$f(X,Y,Z)=[u,v]$f(X,Y,Z)=[u,v], 其中模型里有15 个未知的参数$k_0,k_1,\cdots,k_{14}$k0​,k1​,⋯,k14​
已知：$u_i,v_i \Leftarrow\Rightarrow X_i,Y_i,Z_i$ui​,vi​⇐⇒Xi​,Yi​,Zi​, $i=[1 \cdots N]$i=[1⋯N] 共N组数据对.
求：参数$k_0,k_1,\cdots,k_{14}$k0​,k1​,⋯,k14​

结合opencv 源代码，做简单分析，就不对细节展开：

 while (change > 1e-10 && iter < MaxIter)
    {
        std::vector<Point2d> x;
        Mat jacobians;
        projectPoints(objectPoints, x, rvec, tvec, param, jacobians);// 求解处u,v 对15 个参数的偏导，构成一个 雅克比矩阵。

        Mat ex = imagePoints - Mat(x).t();  // 这里计算的是-F(xⁿ),此处n=k，相当于 -Δy
        ex = ex.reshape(1, 2);

        J = jacobians.colRange(8, 14).clone();//只取外参6 个参数组成的jacobian 矩阵。

        SVD svd(J, SVD::NO_UV);
        double condJJ = svd.w.at<double>(0)/svd.w.at<double>(5);

        if (condJJ > thresh_cond)
            change = 0;
        else
        {
            Vec6d param_innov;
            solve(J, ex.reshape(1, (int)ex.total()), param_innov, DECOMP_SVD + DECOMP_NORMAL); // 上面的第2步，解线性方程组

            Vec6d param_up = extrinsics + param_innov;   / 更新参数，第3 步
            change = norm(param_innov)/norm(param_up);
            extrinsics = param_up;
            iter = iter + 1;                  //继续迭代， 第4步。

            rvec = Mat(Vec3d(extrinsics.val));
            tvec = Mat(Vec3d(extrinsics.val+3));
        }
    }

参考

https://baike.baidu.com/item/牛顿法/1384129?fr=aladdin
https://www.cnblogs.com/cidpmath/articles/6032051.html