点到三角形的最近点--二次规划

问题如下在三角形中找一个点p到三角形内部最近的点(可能在边上,顶点上或者内部)
在这里插入图片描述
这个当然可以用几何的方法用条件判断,这里介绍建模为优化问题

其实这个问题可以表示为
a r g m i n x ∥ A x − p ∥ 2 2 , s . t . ∑ x = 1 , x > = 0 arg min _x \| Ax - p \|_2^2, \quad s.t. \sum_x=1, x>=0 argminxAxp22,s.t.x=1,x>=0
就是三角形内部的点可以用顶点差值表示(重心坐标),这样就转换成了最小二乘问题。

有趣的是其实这个问题的弱化版为 找线段外一点到这个线段最近的位置。我就是通过这个简单的例子最终发现单纯型法算不出来。

求解这个其实可以经过一些推导转化成类似线性规划的问题,但是有个ux=0的情况,表示如果用单纯性算法时,互补松弛变量不能同时作为基变量 (推了很长时间就是无法实现)。

后面展示了用库求解和用有效集的方法求解

1. 用二次规划的库求解

def problem(tri_p, px):
    At = tri_p
    A = At.T
    Atb = At.dot(px)
    AtA = At.dot(A)
    return AtA, Atb
from cvxopt  import solvers, matrix 
tri_p=np.array([
    [0, 0], [2, 0], [1, 2]
], np.float64)
px = np.array([1, 3], np.float64)

# 似乎必须是 float64
P, q = problem(tri_p, px)
P = matrix(P)
q = matrix(-q)
G = matrix(-np.eye(3))
h = matrix(np.zeros(3))
A = matrix(np.ones((1, 3)))
b = matrix(np.array([1.]))

sol = solvers.qp(P,q,G,h,A,b)

#print(sol)
print(sol['x'])

输出如下
在这里插入图片描述

2.(失败) 尝试的单纯型法

这个就不要看了,主要是推导了很长时间,换了很多种方法都没有弄出来。
大体思路就是KKT条件后,构造等式约束,并构造一个目标函数。最后发现达不到最优点

def f5(tri_p, px):
    _A, _b = problem(tri_p, px)
    neg  = _b<0
    if neg.sum()>0:
        _A[neg] *= -1
        _b[neg] *= -1
        
    nall = 3+3+4+2
    A=np.zeros( (4, nall) )
    b=np.zeros( (4) )
    
    A[:3, :3] = _A
    A[:3, 3:3+3] = -np.eye(3)
    
    A[3, :3] = 1
    
    A[:3, 10] = -1
    A[:3, 11] = 1
    A[:4, 6:10] = np.eye(4)
    
    b[:3] = _b
    b[3] = 1
    
    enters = np.arange(6, 10)
    
    slack = np.arange(nall)
    
    slack[:3], slack[3:3+3] = slack[3:3+3].copy(), slack[:3].copy()
    slack[10:12] = 9
    
    z = np.zeros( nall + 1 )
    
    arg_type="max"
    #z[nall-4:-1] = -1
    z[6:10] = -1
    #z[nall-5:-1] = -1
    #z[nall-6] = 1
    
    # p(enters)
    # p(slack)
    # p(A)
    # p(z)
    
    for i in range(4):
        change_A_b(A, b, z, i, enters[i])
    
    # p(A)
    # p(b)
    # p(z)
    # exit()
    f5_solve(A, b, z, enters, (slack), arg_type, n_it=20)
    x = np.zeros( nall )
    x[enters] = b
    p("mx", x)
    return x[:3]


def f5_solve(A, b, z, enters, slack, arg_type, n_it=20):
    for i in  range(n_it):
        if not sel_enter_outer(A, b, z, enters, arg_type, slack): break
        

def sel_enter_outer(A, b, z, enters, arg_type="max", slack=None):
    r, c = A.shape
    slack_mp = None
    if len(slack)==2:
        slack, slack_mp = slack
    vc = np.zeros(c)
    for _c in range(c):
        ci = sel_enter_idx(z[:-1], arg_type, enters, vc)
        if ci < 0: return False
        vc[ci] = 1
        ri = sel_outer_idx(A[:, ci], b)
        if ri < 0: continue
        #p("ci, ri", ci, enters[ri])
        if slack is None: break
        else:
            if slack[ci]==enters[ri] or slack[ci] not in enters: 
                if slack_mp and ci in slack_mp:
                    if slack[ci]==enters[ri]:
                        if slack_mp[ci] not in enters: break
                        else: continue
                    else:
                        if slack_mp[ci]==enters[ri] or slack_mp[ci] not in enters: break
                else: break
    if _c==c: return False
    p("ci, ri, z", ci, enters[ri], z)
    change_A_b(A, b, z, ri, ci)
    enters[ri] = ci
    p("ck---")
    p(enters)
    #p(A)
    #p(b)
    p(z)
    
    return True

def change_A_b(A, b, z, ri, ci):
    b[ri] /= A[ri, ci]
    A[ri] /= A[ri, ci].copy()
    for i in range(len(b)):
        if i!=ri:
            b[i] -= A[i, ci]*b[ri]
            A[i] -= A[i, ci]*A[ri]
    z[-1] -= z[ci]*b[ri]
    z[:-1] -= z[ci]*A[ri]
    
    
# 0 是可以找的    
def sel_enter_idx(z, arg_type, enters, vc):
    if arg_type=="max": mm = -1
    elif arg_type=="min": mm = 1
    _i = -1
    for i in range(len(z)):
        if vc[i]: continue # 不合法的直接不要
        if i in enters: continue
        if arg_type=="max":
            if z[i]<=0: continue
            if mm < z[i]:
                mm = z[i]
                _i = i
        else:
            if z[i]>=0: continue
            if mm > z[i]:
                mm = z[i]
                _i = i
    return _i
    
def sel_outer_idx(a_c, b):
    mx=np.inf
    _i = -1
    for i in range(len(b)):
        if a_c[i]>0 and mx > (b[i]/a_c[i]):
            mx=b[i]/a_c[i]
            _i = i
    return _i

3. 有效集法 【本次最值得记录的】

https://wenku.baidu.com/view/ad34f36079563c1ec5da71fc.html
其实之前感觉弄不出来的时候就想到找了,一开始感觉很复杂,所以不想看。后面仔细看了一下,感觉也很好了解。
整体思路就类似,高斯牛顿法,给个初始值,然后求dp,但是这里主要多了个保证在可行域之中。

里面应该求解那里有重复计算的地方,这里就不优化了。

def f7(tri_p, px):
    A, b = problem(tri_p, px)
    n_x = 3
    n_eq = 1
    n_neq = n_x
    n_max_Q = n_x + n_eq + n_neq # 2 个变量, 1个等式变量,最多2个不等式变量
    valid = np.zeros(n_max_Q, np.bool)
    Q = np.zeros( (n_max_Q, n_max_Q) )
    Ae = np.zeros( (n_eq, n_x) ) + 1 # task 有关
    be = np.zeros( (n_eq) ) + 1
    #Ane = np.zeros( (n_neq, n_x) )
    Ane = np.eye( n_x )
    bne = np.zeros( (n_neq) )
    
    Q[:n_x, :n_x] = A
    Q[:n_x, n_x:n_x+n_eq] = -Ae.T
    Q[n_x:n_x+n_eq, :n_x] = Ae
    Q[:n_x, n_x+n_eq:] = -Ane.T
    Q[n_x+n_eq:, :n_x] = Ane
    
    q = np.zeros( n_max_Q )
    q[:n_x] = b
    q[n_x:n_x+n_eq] = be
    q[n_x+n_eq:] = bne
    
    # p(Q)
    # p(q)
    # exit()
    
    valid[:n_x+n_eq] = True
    
    n_must = n_x+n_eq
    # task 有关
    x_init=np.zeros(n_x) + 1/n_x
    oA, ob = A, b
    
    #q[n_x:] = 0 # 才发现根本没有用
    bi = n_x + n_eq
    _cnt=0
    while True:
        p("x:", x_init)
        d_f_x_k = oA.dot(x_init) - ob
        q[:n_x] = -d_f_x_k # 修改了初始值,则重新构造求解 dx 的 b
        
        A = Q[valid][:, valid]
        b = q[valid].copy()
        b[n_x:] = 0
        
        # 有重复计算
        AtA = A.T.dot(A)
        Atb = A.T.dot(b)
        
        d = np.linalg.solve(AtA, Atb)
        #_cnt+=1
        #if _cnt>10: exit()
        p(_cnt, np.abs(d[:n_x]).sum())
        # 达到极值
        if np.abs(d[:n_x]).sum() < 1e-6:
            if d.shape[0] == bi: break # 所有等式不等式都满足了,
            mm = np.min(d[bi:])
            if mm>=0: break
            # 把最不能满足不等式约束的去掉
            valid[bi:][valid[bi:]][np.argmin(d[bi:])] = False
            
        else:
            # 不为0,基本就说明约束不够, 达不到极值
            # step 1:保证安求出的下降方向更新后,未参与的约束合法
            alpha = 1
            # 【只针对不等式约束】
            for i in range(bi, bi+n_neq):
                if not valid[i]:
                    # 对于不在可行解之中的
                    fm = Q[i, :n_x].dot(d[:n_x])
                    if fm<0: # 可能会出了可行域, 因为不等式是 >= b, 所以如果fm小于0则可能 出现<b
                        # a^T(x+\alpah d) = b -> \alpha = (b-a^Tx)/
                        alpha = min(alpha, (q[i]-Q[i, :n_x].dot(x_init))/fm)
                        assert alpha!=0

            x_init += alpha*d[:n_x] # 更新当前解
            
            # step2: 发现有新的有效约束则加入,也是不等式中
            for i in range(bi, bi+n_neq):
                if not valid[i]:
                    if np.abs(q[i]-Q[i, :n_x].dot(x_init))<1e-9:
                        valid[i] = True
            
            #p("alpha", alpha, d)
            
    p(x_init) 
    return x_init

4. 调试时的其他代码

4.1 杂记

def plot_triangle(A, B, C):
    x = [A[0], B[0], C[0], A[0]]
    y = [A[1], B[1], C[1], A[1]]
    plt.plot(x, y, linewidth=2)

def f_cmp(tri_p, px, x1, x2):
    r1 = _cac(tri_p, px, x1)
    r2 = _cac(tri_p, px, x2)
    p(r1, r2)
    abs = np.abs( r1 - r2 ).sum()
    return abs<1e-6

def plot_line(A, B):
    x = [A[0], B[0]]
    y = [A[1], B[1]]
    plt.plot(x, y, linewidth=2)

4.2 关于线段和点的

在这里插入图片描述

plt.gca().set_aspect('equal', adjustable='box')
plot_line(tri_p[0], tri_p[1])
plt.plot( px[0], px[1], "r." )
plt.plot( tri_p[0][0], tri_p[0][1], "g*" )
plt.plot( tri_p[1][0], tri_p[1][1], "y+" )
p1 = tri_p.T.dot(x1)
p2 = tri_p.T.dot(x2)
plt.plot( p1[0], p1[1], "b*" )
plt.plot( p2[0], p2[1], "r*" )
plt.show()
# 可以解析求解, 利用余弦函数的。 x = |p21||px1|cos / |p21|^2
def f6(tri_p, px):
    p1 = tri_p[0]
    p2 = tri_p[1]
    p21 = p2 - p1
    px1 = px - p1
    x = (p21*px1).sum()/(p21*p21).sum()
    if x<0: x=0
    if x>1: x=1
    return np.array([1-x, x])

# 也可以用有效集的方法
def f7(tri_p, px):
    A, b = problem(tri_p, px)  # AtA, Atb
    n_x = 2 # 未知数变量
    n_eq = 1 # 1 等式 
    n_neq = n_x
    n_max_Q = n_x + n_eq + n_neq # 2 个变量, 1个等式变量,最多2个不等式变量
    valid = np.zeros(n_max_Q, np.bool)
    Q = np.zeros( (n_max_Q, n_max_Q) )
    Ae = np.zeros( (n_eq, n_x) ) + 1 # task 有关
    be = np.zeros( (n_eq) ) + 1
    #Ane = np.zeros( (n_neq, n_x) )
    Ane = np.eye( n_x ) 
    bne = np.zeros( (n_neq) )
    
    Q[:n_x, :n_x] = A
    Q[:n_x, n_x:n_x+n_eq] = -Ae.T
    Q[n_x:n_x+n_eq, :n_x] = Ae
    Q[:n_x, n_x+n_eq:] = -Ane.T
    Q[n_x+n_eq:, :n_x] = Ane
    
    q = np.zeros( n_max_Q )
    q[:n_x] = b
    q[n_x:n_x+n_eq] = be
    q[n_x+n_eq:] = bne
    
    # p(Q)
    # p(q)
    # exit()
    
    # 上面的Q,q是使用所有的等式,通过valid决定具体用哪个?
    valid[:n_x+n_eq] = True
    
    n_must = n_x+n_eq
    # task 有关
    x_init=np.zeros(n_x) + 1/n_x
    oA, ob = A, b
    
    #q[n_x:] = 0 # 才发现根本没有用
    bi = n_x + n_eq
    _cnt=0
    while True:
        p("x:", x_init)
        d_f_x_k = oA.dot(x_init) - ob
        q[:n_x] = -d_f_x_k
        
        A = Q[valid][:, valid]
        b = q[valid].copy()
        b[n_x:] = 0
        
        # 有重复计算
        AtA = A.T.dot(A)
        Atb = A.T.dot(b)
        
        d = np.linalg.solve(AtA, Atb)
        #_cnt+=1
        #if _cnt>10: exit()
        p(_cnt, np.abs(d[:n_x]).sum())
        if np.abs(d[:n_x]).sum() < 1e-6:
            if d.shape[0] == bi: break
            mm = np.min(d[bi:])
            if mm>=0: break
            # 把最不能满足不等式约束的去掉
            valid[bi:][valid[bi:]][np.argmin(d[bi:])] = False
            # cnt=0
            # mm = 0
            # sel = None
            # for i in range(bi, bi+n_neq):
                # if valid[i]:
                    # if d[bi+cnt]<mm: 
                        # mm = d[bi+cnt]
                        # sel = i
                    # cnt+=1
            # if mm == 0: break
            # valid[i] = False # 最小的那个去掉
        else:
            # step 1:保证更新之后,未参与的约束合法
            alpha = 1
            for i in range(bi, bi+n_neq):
                if not valid[i]:
                    # 对于不在可行解之中的
                    fm = Q[i, :n_x].dot(d[:n_x])
                    if fm<0: # 可能会出了可行域, 因为不等式是 >= b, 所以如果fm小于0则可能 出现<b
                        # a^T(x+\alpah d) = b -> \alpha = (b-a^Tx)/
                        alpha = min(alpha, (q[i]-Q[i, :n_x].dot(x_init))/fm)
                        assert alpha!=0

            x_init += alpha*d[:n_x]
            
            # step2: 发现有新的有效约束则加入
            for i in range(bi, bi+n_neq):
                if not valid[i]:
                    if np.abs(q[i]-Q[i, :n_x].dot(x_init))<1e-9:
                        valid[i] = True
            
            #p("alpha", alpha, d)
            
    p(x_init) 
    return x_init

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值