Rigid3DTransform and Euler3DTransform
It is not clear, I appreciate if someone explains the difference.
Another question can someone check my computation bellow is correct:
import numpy as np
import sys, math
print("******************************************************")
print (" Points Transformation ")
print("******************************************************")
np.set_printoptions(precision=2)
np.set_printoptions(suppress=True)
print("---------------------------------------------------")
print(" Define input ")
print("---------------------------------------------------")
# Input Points
p=np.array([[-1.1499 , 4.96146 ,-3.73897 ],[ 1.12107 , 6.15124 ,-4.09615 ], [ 2.56265 , 7.01562 ,-4.0416 ], [ 4.1089 , 7.87024 ,-3.37502 ], [ 5.23609 , 8.08297 ,-1.92341 ], [ 5.37945 , 7.84559 , 0.0562932], [ 4.72791 , 6.72648 , 1.12291 ], [ 3.17306 , 5.88972 , 1.40773 ], [ 2.09212 , 5.46035 , 0.298082], [ 1.39364 , 5.93 ,-0.860716], [ 1.80359 , 6.98823 ,-1.9811 ], [ 2.79237 , 8.16489 ,-1.89504 ], [ 3.54371 , 8.4603 ,-0.688138], [ 3.18755 , 8.01944 , 0.458005], [ 1.99834 , 7.59242 , 0.468096], [ 1.50188 , 7.8538 ,-0.489043], [ 2.28302 , 8.5749 ,-0.594259]])
print(p)
# 3DEuler parameters
ep=np.array([ -0.369236, -0.074973, 0.177428, -0.895437, -7.486193, -9.989277])
print("---------------------------------------------------")
print(" Parameters to Rotation Matrix ")
print("---------------------------------------------------")
[rx,ry,rz]=ep[0:3];
[tx,ty,tz]=ep[3:6];
Ms = []
cosx = math.cos(rx)
sinx = math.sin(rx)
Ms.append(np.array([[1, 0 , 0 ],
[0, cosx, -sinx],
[0, sinx, cosx]]))
cosy = math.cos(ry)
siny = math.sin(ry)
Ms.append(np.array( [[cosy , 0, siny],
[0 , 1, 0 ],
[-siny, 0, cosy]]))
cosz = math.cos(rz)
sinz = math.sin(rz)
Ms.append(np.array([[cosz, -sinz, 0],
[sinz, cosz, 0],
[0 , 0 , 1] ]))
if Ms:
mt = reduce(np.dot, Ms[::-1])
# convert to 4x4
m= np.zeros((4,4)) ; m[3,3]=1 ; m[0:3,0:3]=mt
# add translations
m[0:3,3]=[tx,ty,tz]
else:
m= np.eye(3)
print("---------------------------------------------------")
print(" displacement field ")
print("---------------------------------------------------")
# get the displacement vector = len(p)
d=np.zeros((len(p),4))
#print(p_dis)
for i in range(0,len(p)):
dt=np.ones(4); dt[0:3] = dt[0:3] * p[i]
#print(dt)
np.matmul(m,dt,d[i])
print(d[i])
print("---------------------------------------------------")
print(" new transformed points ")
print("---------------------------------------------------")
# get the new points locations
pn=np.zeros((len(p),3))
for i in range(0,len(p)):
pn[i]= p[i] + d[i][0:3]
print(pn[i])