|
楼主 |
发表于 2023-9-24 17:31:41
|
显示全部楼层
- import numpy as np
- class coordinatetransfer:
- #first point
- def getRFromPoints(self,points_old,points_new):
- #get lambda
- part_upper = np.sqrt(np.sum((points_new[1] - points_new[0]) ** 2))
- part_down = np.sqrt(np.sum((points_old[1] - points_old[0]) ** 2))
- lamda = part_upper / part_down
- #get A
- points_dif_old = points_old[1:-1] - points_old[0]
- points_dif_new = points_new[1:-1] - points_new[0]
- A = np.empty((0, 3))
- for n in range(points_dif_old.shape()[0]):
- A = np.append(A, [[0, -lamda *points_dif_old[n][2] - points_dif_new[n][2],
- -lamda *points_dif_old[n][1] - points_dif_new[n][1]]
- , [-lamda * points_dif_old[n][2] - points_dif_new[n][2], 0,
- -lamda * points_dif_old[n][0] - points_dif_new[n][0]]
- ,[-lamda * points_dif_old[n][1] - points_dif_new[n][1],
- -lamda * points_dif_old[n][0] - points_dif_new[n][0], 0]]
- ,axis = 0)
- print (A)
- #get L
- points_dif_old = points_old[1:-1] - points_old[0]
- points_dif_new = points_new[1:-1] - points_new[0]
- L = np.empty((0, 3))
- for n in range(points_dif_old.shape()[0]):
- L = np.append( [L, points_dif_new[n][0]-lamda * points_dif_old[n][0]
- ,points_dif_new[n][1]-lamda * points_dif_old[n][1]
- ,points_dif_new[n][2] - lamda * points_dif_old[n][2]])
- L=L.T
- print(L)
- #get x
- X=np.linalg.inv(A.T*A)*A.T*L
- print(X)
- X1=X.T
- a = X[0]
- b = X[1]
- c = X[2]
- #get R
- x1=1+a*a+b*b+c*c
- x2=np.array([[1+a*a-b*b-c*c,-2*c-2*a*b,-2*b+2*a*c],
- [2*c-2*a*b,1-a*a+b*b-c*c,-2*a-2*b*c],
- [2*b+2*a*c,2*a-2*b*c,1-a*a-b*b+c*c]])
- R=1/x1*x2
- print(R)
- return R,lamda
复制代码 |
|