迭代法求模型的参数

迭代优化：

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

问题描述：

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

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

• S1:先给$k$一个任意的初始值$k_0$，正向计算$g(k_0)=x_0^3{k}_0^2-y_0$,注意：这里的$g(k_0)=f(x_0)-y_0$,就是原函数的$\Delta y$
• S2:计算$g^{{}^{\prime }}(k)=2x_0^3{k}_0$在$k_0$处的导数。
• S3:计算$\Delta k=\frac{g(k_0)}{g^{{}^{\prime }}(k_0)}$.
• 更新$k=k_0-\Delta k$
• 重复S1-S3 直到$\Delta 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)$ 关于 $x$ 的 Jacobi 矩阵$\mathbf{J}(\mathbf{x})=(\frac{\partial \mathbf{F}}{\partial \mathbf{x}})$ 可逆时, 有
${\mathbf{x}}^{(k+1)}={\mathbf{x}}^{(k)}-{\mathbf{J}}^{-1}({\mathbf{x}}^{(k)})\mathbf{F}({\mathbf{x}}^{(k)}),$,

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

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

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

结合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