基于Gekko实现相机镜头畸变校准的可逆性约束优化

我正在用Gekko求解相机镜头畸变校准的最优解，变量组为K1、K2或K1、K2、CX、CY。此前用scipy.optimize.minimize得到了合理结果，但因最终结果依赖复杂的可逆性约束，无法在Scipy中实现，现寻求在Gekko中建模该约束的方法。

可逆性约束定义 #

约束逻辑由div_constraint_invertibility()实现，依赖以下辅助函数：

辅助函数1：计算归一化系数 #

def get_rad_norm_k(dc, w, h):
      dmi = get_max_dist(dc=dc, w=w, h=h)
      r2 = m.sqrt(dmi) / 2
      r2_2 = r2*r2
      r2_4 = r2_2*r2_2
      denk1 = -12 * r2_2
      denk2 = -12 * r2_4
      return denk1, denk2

辅助函数2：计算最大距离 #

def get_max_dist(dc, h, w):
    corner_distances = [(dc[0] - 0)**2 + (dc[1] - 0)**2,
                        (dc[0] - 0)**2 + (dc[1] - w)**2,
                        (dc[0] - h)**2 + (dc[1] - w)**2,
                        (dc[0] - h)**2 + (dc[1] - 0)**2]

    mx = corner_distances[0]
    for cd in corner_distances[1:]:
        mx = m.max3(mx, cd)
    return mx

核心约束函数 #

参数x为[P1,P2]或[P1,P2,CX,CY]，其中P1、P2是K1、K2的归一化值：

def div_constraint_invertibility(x, ldm):

    p1 = x[0]
    p2 = x[1]
    dc = [ldm.Cx, ldm.Cy]
    
    w = ldm.width
    h = ldm.height
    denk1, denk2 = get_rad_norm_k(dc=dc, w=w, h=h)
    
    r1sq = get_max_dist(dc=dc, w=w, h=h)
    r1p4 = r1sq**2
    
    k1 = (((-p1) / (1 + p1)) + ((16 * p2) / (1 + p2))) / denk1
    k2 = (((-4 * p2) / (1 + p2)) + (p1 / (1 + p1))) / denk2
    
    if -2 < r1sq * k1 < 2:
        if -1. - r1sq * k1 < r1p4 * k2 < (1. - r1sq * k1) / 3:
            return 1  # 约束满足
    else:
        if r1sq * k1 >= 2:
            if -1. - r1sq * k1 < r1p4 * k2 < (-r1p4 * k1**2 / 12):
                return 1  # 约束满足
    
    return -1  # 约束不满足

我已尝试用Gekko函数实现相关逻辑，下一步打算用m.if3()实现上述条件判断，但嵌套m.if3()不够优雅，且不确定约束函数应返回何种值（不应是-1或1，需返回符合Gekko要求的约束函数值）。

编辑1：Scipy实现参考 #

以下是Scipy最小化的实现方式，目标函数cumulative_point_to_line_error提取畸变图像中的弧点，应用畸变模型计算去畸变后点的直线拟合误差。初始点示例：

dm0 = [-0.0, 0.0, input_image_center_X, input_image_center_Y]

同时包含对应弧的点数组（例如某图像可能有30组提取弧的点列表）：

def model_estimation(ip, w, h, oc):
    ldm = ip.lens_model
    dc = [ldm.Cx, ldm.Cy]
    prevk1 = ldm.d[1]

    if len(ldm.d) != 3:
        ldm.d.append(0)
    prevk2 = ldm.d[2]
    if oc:
        dm0 = [prevp1, prevp2, dc[0], dc[1]]
    else:
        dm0 = [prevp1, prevp2]

    cons = []
    bnds = []
    if oc:
        dc_drift = 2
        bnds = [(-np.inf, np.inf), (-np.inf, np.inf),
                (float(ldm.width / 2 - dc_drift), float(ldm.width / 2 + dc_drift)),
                (float(ldm.height / 2 - dc_drift), float(ldm.height / 2 + dc_drift))]

    cons.append({'type': 'ineq', 'fun': div_constraint_invertibility, 'args': (ip.lens_model,)})

    print(f"Optimizing: {dm0}")
    if cons:
        print(f"Constraints:")
        for con in cons:
            print(f"Const: {con['fun'].__name__}")
    if bnds:
        print(f"Bounds:")
        for idx, bnd in enumerate(bnds):
            print(f"{bnd[0]} <= x{idx} <= {bnd[1]}")

    result = spo.minimize(fun=cumulative_point_to_line_error, x0=np.array(dm0), args=(ip,),
                          constraints=cons, bounds=bnds, method='SLSQP', options={"disp": True})

    print(f"Optimization Result: {result.x} | {result.fun}")

编辑2：Gekko目标函数转换尝试 #

我正在将目标函数转换为Gekko可用形式，疑问是否可对cumulative_point_to_line_error调用m.Minimize()。目前正在处理div_update_point中的除零检查，以及dist_points_to_equation中的条件判断：

def dist_points_to_equation(m, k1, k2, dc_x, dc_y, hl: Line):
    pnts_o = hl.pnts
    err = 0
    zero = 10e-100
    num_pnts = len(pnts_o)
    um = vm = 0
    suu = suv = svv = 0

    pnts_e = []
    for pnt in pnts_o:
        x, y = div_update_point(m=m, px=pnt[0], py=pnt[1], K1=k1, K2=k2, dc_x=dc_x, dc_y=dc_y)
        pnts_e.append([x, y])

    for pnt in pnts_e:
        x = pnt[0]
        y = pnt[1]
        um += x
        vm += y
    um = um / num_pnts
    vm = vm / num_pnts

    for pnt in pnts_e:
        x = pnt[0]
        y = pnt[1]
        suu += (x - um) ** 2
        suv += (x - um) * (y - vm)
        svv += (y - vm) ** 2
    suu = suu / num_pnts
    suv = suv / num_pnts
    svv = svv / num_pnts

    if np.abs(suv) <= zero:
        # print('XY <= ZERO*')
        if suu < svv and svv > zero:
            hl.a = 1.
            hl.b = 0.
            hl.c = -um
            err = 0.
            # print(f'Iter. Method Line: {self.a}x + {self.b}y + {self.c} = 0')
            # print(f'Error == {err}')
            return hl, err
        if svv < suu and suu > zero:
            hl.a = 0.
            hl.b = 1.
            hl.c = -vm
            err = 0
            # print(f'Iter. Method Line: {self.a}x + {self.b}y + {self.c} = 0')
            # print(f'Error == {err}')
            return hl, err
        # print('Failed to recalculate!')
        return None, err

    r = np.array([[1, 0, 0],
                  [1, 0, 0],
                  [0, 1, 0],
                  [0, 1, 0]], dtype=float)

    h = 0.5 * (suu - svv) / suv
    if h > 0:
        r[0, 1] = -h - np.sqrt(1. + np.square(h))
        r[0, 2] = -(um + r[0, 1] * vm)
        r[1, 1] = -1. / r[0, 1]
        r[1, 2] = -(um + r[1, 1] * vm)
        r[2, 0] = h + np.sqrt(1. + np.square(h))
        r[2, 2] = -(r[2, 0] * um + vm)
        r[3, 0] = -1. / r[2, 0]
        r[3, 2] = -(r[3, 0] * um + vm)
    else:
        r[0, 1] = -h + np.sqrt(1. + np.square(h))
        r[0, 2] = -(um + r[0, 1] * vm)
        r[1, 1] = -1. / r[0, 1]
        r[1, 2] = -(um + r[1, 1] * vm)
        r[2, 0] = h - np.sqrt(1. + np.square(h))
        r[2, 2] = -(r[2, 0] * um + vm)
        r[3, 0] = -1. / r[2, 0]
        r[3, 2] = -(r[3, 0] * um + vm)

    for row in range(0, r.shape[0]):
        norm = np.sqrt(np.square(r[row, 0]) + np.square(r[row, 1]))
        for col in range(0, r.shape[1]):
            r[row, col] /= norm

    min_err = 0
    k = 0
    for pnt in pnts_e:
        x = pnt[0]
        y = pnt[1]
        sol = r[0, 0] * x + r[0, 1] * y + r[0, 2]
        min_err += np.square(sol)

    for row in range(1, r.shape[0]):
        h = 0
        for pnt in pnts_e:
            x = pnt[0]
            y = pnt[1]
            sol = r[row, 0] * x + r[row, 1] * y + r[row, 2]
            h += np.square(sol)
        if h < min_err:
            k = row
            min_err = h

    hl.a = r[k, 0]
    hl.b = r[k, 1]
    hl.c = r[k, 2]
    # err = min_err / num_pnts
    # print(min_err, num_pnts)
    # print(err)

    return hl, min_err


def cumulative_point_to_line_error(m, p1, p2, dc_x, dc_y, w, h, houghlines):

    denk1, denk2 = get_rad_norm_k(m, dc_x=dc_x, dc_y=dc_y, w=w, h=h)

    k1 = m.Intermediate(((-p1) / (1 + p1)) + (16 * p2 / (1 + p2))/denk1)
    k2 = m.Intermediate(((-4 * p2) / (1 + p2)) + (p1 / (1 + p1))/denk2)

    cumulative_err = 0
    for idx, hl in enumerate(houghlines):
        new_hl, err = dist_points_to_equation(m, k1, k2, dc_x, dc_y, hl)
        houghlines[idx] = new_hl
        cumulative_err += err
    return cumulative_err

内容的提问来源于stack exchange，提问作者SassyCat