import numpy as np
from numpy.linalg import *
# 基本输入：当前坐标t.x ,t.y ,控制点p ,控制点结果q
def MLS(t, p, q):
#p = np.array([[5, 5], [1, 1], [3, 4], [4, 7], [4, 4]])
#q = np.array([[5, 5], [1, 1], [4, 4], [4, 7], [4, 4]])
list_p = p.tolist()
list_q = q.tolist()
list_t = t.tolist()
if list_t in list_p:
for it in range(5):
if list_t == list_p[it]:
return np.array(list_q[it])
# 计算w
w = 1/np.sum(np.square(p-t),axis=1)
w_sum = np.sum(w)
# 计算p*,q*
p_h_1 = p[:, 0]
p_h_2 = p[:, 1]
p_up = w * p_h_1
p_down = w * p_h_2
p_count = np.stack((p_up, p_down))
q_h_1 = q[:, 0]
q_h_2 = q[:, 1]
q_up = w * q_h_1
q_down = w * q_h_2
q_count = np.stack((q_up, q_down))
px = np.sum(p_count,axis=1)/w_sum
qx = np.sum(q_count,axis=1)/w_sum
# 计算p^,q^
pc = p - px
qc = q - qx
# 计算Aj
A = t-px
B = np.zeros((2,2),"float64")
pc_x_qc = np.zeros((2,2),"float64")
i = 0
for item_p in pc:
B += w[i] * np.array([[item_p[0] * item_p[0], item_p[0] * item_p[1]]
, [item_p[1]*item_p[0], item_p[1]*item_p[1]]
])
i += 1
if matrix_rank(B) == 2: # 矩阵的秩
# 矩阵的逆
C = inv(B)
# pc qc
for item in range(5):
pc_x_qc += w[item] * np.array([[pc[item][0] * qc[item][0], pc[item][0] * qc[item][1]]
, [pc[item][1]*qc[item][0], pc[item][1]*qc[item][1]]])
M= np.dot(C,pc_x_qc)
ZAjqj = np.dot(A,M)
# 计算fv
fv = ZAjqj + qx
return fv
else:
print("----------------------------")
return t
# 基本输入：当前坐标t.x ,t.y ,控制点p ,控制点结果q
def MLS_rigid(t, p, q):
#p = np.array([[5, 5], [1, 1], [3, 4], [4, 7], [4, 4]])
#q = np.array([[5, 5], [1, 1], [4, 4], [4, 7], [4, 4]])
kev = np.array([[0, 1], [-1, 0]])
list_p = p.tolist()
list_q = q.tolist()
list_t = t.tolist()
if list_t in list_p:
for i,list_p_item in enumerate(list_p):
if list_t == list_p_item:
return np.matrix(list_q[i])
# 计算w
w = 1/np.sum(np.square(p-t),axis=1)
w_sum = np.sum(w)
# if w_sum < 0.0005 :
# return np.matrix(t)
# 计算p*,q*
p_h_1 = p[:, 0]
p_h_2 = p[:, 1]
p_up = w * p_h_1
p_down = w * p_h_2
p_count = np.stack((p_up, p_down))
q_h_1 = q[:, 0]
q_h_2 = q[:, 1]
q_up = w * q_h_1
q_down = w * q_h_2
q_count = np.stack((q_up, q_down))
px = np.sum(p_count,axis=1)/w_sum
qx = np.sum(q_count,axis=1)/w_sum
# 计算p^,q^
pc = p - px
qc = q - qx
# v-p*
v_px = t - px
# 计算Aj
# Ai = wi x p^ x v-p*
# fr(v) = q^ x Ai
# Aj = wi x q^ x p^
# fr = Aj x v-p*
# (x,y)L = (-y,x) ==>(x,y)*[[0,1],[-1,0]]
Aj = np.zeros((1, 2), "float64")
v_px_t = 0 - np.matrix(v_px)*kev
v_px_con = np.stack((np.matrix(v_px), v_px_t))
for i in range(len(list_p)):
pct = 0 - np.matrix(pc[i])*kev
pc_con = np.stack((np.matrix(pc[i]), pct))
Bj = w[i]*np.matrix(qc[i]) * pc_con
Aj += Bj
fr = Aj * (v_px_con.T)
# 求行列式det()/这里的|A-B|表示的是距离不是行列式的值
# fr(v) = |v− p∗|(~fr(v)/|~fr(v)|) + q∗
d_fr = np.sqrt(np.sum(np.square(fr)))
d_v_px = np.sqrt(np.sum(np.square(v_px)))
frv = (d_v_px / d_fr) * fr + qx
return frv
# 基本输入：当前坐标t.x ,t.y ,控制点p ,控制点结果q
def MLS_similary(t, p, q):
#p = np.array([[5, 5], [1, 1], [3, 4], [4, 7], [4, 4]])
#q = np.array([[5, 5], [1, 1], [4, 4], [4, 7], [4, 4]])
kev = np.array([[0, 1], [-1, 0]])
error_point = 0
list_p = p.tolist()
list_q = q.tolist()
list_t = t.tolist()
if list_t in list_p:
for it in range(5):
if list_t == list_p[it]:
return np.matrix(list_q[it])
# 计算w
w = 1/np.sum(np.square(p-t),axis=1)
w_sum = np.sum(w)
# 计算p*,q*
p_h_1 = p[:, 0]
p_h_2 = p[:, 1]
p_up = w * p_h_1
p_down = w * p_h_2
p_count = np.stack((p_up, p_down))
q_h_1 = q[:, 0]
q_h_2 = q[:, 1]
q_up = w * q_h_1
q_down = w * q_h_2
q_count = np.stack((q_up, q_down))
px = np.sum(p_count,axis=1)/w_sum
qx = np.sum(q_count,axis=1)/w_sum
# 计算p^,q^
pc = p - px
qc = q - qx
v_px = t - px
Aj = np.zeros((1, 2), "float64")
us = 0
# 计算Aj
# (x,y)L = (-y,x) ==>(x,y)*[[0,1],[-1,0]]
v_px_t = 0 - np.matrix(v_px)*kev
v_px_con = np.stack((np.matrix(v_px), v_px_t))
for i in range(5):
pct = 0 - np.matrix(pc[i])*kev
# qct = 0 - np.matrix(qc[i])*kev
pc_con = np.stack((np.matrix(pc[i]), pct))
# qc_con = np.stack((np.matrix(qc[i]), qct))
Aj += w[i]*np.matrix(qc[i]) * pc_con
us += w[i] * np.multiply(pc[i],pc[i]).sum()
f = Aj * 1 / us * (v_px_con.T) + qx
return f
