问题描述
我想将给定的点云旋转到xy平面中。因此,我通过点云安装了一架飞机。这样,我想计算获得该平面以及xy平面中的点云所需旋转的角度。飞机的中心位于原点。所以我需要绕x轴和y轴旋转。但是我不确定如何继续。有任何想法吗?
到目前为止,这是我的代码:
def fitPlaneLTSQ(df):
(rows,cols) = df.shape
G = np.ones((rows,3))
G[:,0] = df['X']
G[:,1] = df['Z']
Z = df['Y']
(a,b,c),resid,rank,s = np.linalg.lstsq(G,Z)
normal = (a,-1)
nn = np.linalg.norm(normal)
normal = normal / nn
return (c,normal)
#load data
data = pd.read_csv('data.csv',sep=';')
# calc middle of cloud
meanx = np.mean(data['X'])
meany = np.mean(data['Y'])
meanz = np.mean(data['Z'])
#translate cloud to orign
data['X'] = data['X'] - meanx
data['Y'] = data['Y'] - meany
data['Z'] = data['Z'] - meanz
#calc plane
maxx = np.max(data['X'])
maxz = np.max(data['Z'])
minx = np.min(data['X'])
minz = np.min(data['Z'])
c,normal = fitPlaneLTSQ(data)
point = np.array([0.0,0.0,c])
d = -point.dot(normal)
# compute needed points for plane plotting
xx,yy = np.meshgrid([minx,maxx],[minz,maxz])
z = (-normal[0]*xx - normal[1]*yy - d)*1. / normal[2]