toa_3D_bundle_with_smoother.py 文件源码

def bundletoa(D, I, J, xt, yt, debug=1, opts=[]):
    res = None
    jac = None

    for kkk in range(0, 10):

        res, jac = calcresandjac(D, I, J, xt, yt, opts)

        dz = linalg.lstsq(-((jac.conj().T) * jac + 0.1 *
                            np.eye(jac.shape[1])), (jac.conj().T) * res)

        xtn, ytn = updatexy(xt, yt, dz)
        res2, jac2 = calcresandjac(D, I, J, xt, yt, opts)

        cc = np.linalg.norm(jac * dz) / np.linalg.norm(res)

        if np.linalg.norm(res) < np.linalg.norm(res2):

            if cc > 1e-4:

                kkkk = 1
                while (kkkk < 50) and (
                        np.linalg.norm(res) < np.linalg.norm(res2)):
                    dz = dz / 2
                    xtn, ytn = updatexy(xt, yt, dz)
                    res2, jac2 = calcresandjac(D, I, J, xtn, ytn, opts)
                    kkkk = kkkk + 1

        if debug:
            aa_1 = np.linalg.norm(res)
            aa_2 = np.linalg.norm(res + jac * dz)
            aa_3 = np.linalg.norm(res2)
            aa = np.concatenate((aa_1, aa_2, aa_3), 1)

            bb = aa
            bb = bb - bb[1]
            bb = bb / bb[0]

            cc = np.linalg.norm(jac * dz) / np.linalg.norm(res)

            print(aa, bb, cc)

        if np.linalg.norm(res2) < np.linalg.norm(res):

            xt = xtn
            yt = ytn
        else:

            if debug:
                print(kkk, ' stalled')

    xopt = xt
    yopt = yt

    return xopt, yopt, res, jac