As shown in the attached picture, 4 Points are plotted in the world frame. 3 of them (green colored) defines my plane. With the given 3 green points I can calculate the nominal axis for my plane. My expectation is to take one of the 3 green points as origin, calculate nominal vector for the plane. Find another 2 basis vectors. After I have 3 basis vectors defining my local frame, I would like to translate and rotate these points. At the end I would like to make Visualisation where plane is parallel to new local frame.
Here is a function in python that generates/plots the points and plane.
class Point:
x = 0
y = 0
z = 0
def __init__(self,xx,yy,zz):
self.x=xx
self.y=yy
self.z=zz
def change_x(self,new_value):
self.x = new_value
def change_y(self,new_value):
self.y = new_value
def change_z(self,new_value):
self.z = new_value
def change_xyz(self,v_x,v_y,v_z):
self.x = v_x
self.y = v_y
self.z = v_z
def get_x(self):
return self.x
def get_y(self):
return self.y
def get_z(self):
return self.z
def find_plane_equation_with_distance_error(p1, p2, p3, p_new,bPlot = False):
"p* are the points that are used to define a plane"
p12 = [p2.get_x() - p1.get_x(), p2.get_y() - p1.get_y(), p2.get_z() - p1.get_z()]
p13 = [p3.get_x() - p1.get_x(), p3.get_y() - p1.get_y(), p3.get_z() - p1.get_z()]
p_temp = np.array([p1.x,p1.y,p1.z])
unit_vector = np.cross(p12,p13)
toBeDivided = np.sqrt(unit_vector[0]*unit_vector[0]+unit_vector[1]*unit_vector[1]+unit_vector[2]*unit_vector[2])
p_error = (unit_vector[0]*(p_new.get_x() - p1.get_x()) + unit_vector[1]*(p_new.get_y() - p1.get_y()) + unit_vector[2]*(p_new.get_z() - p1.get_z()))
#print('Error in plane fit is [before] : ',p_error)
p_error = p_error / toBeDivided
#print('Error in plane fit is [after] : ',p_error)
#plot the plane
xx, yy = np.meshgrid(range(10), range(10))
d = -p_temp.dot(unit_vector)
# calculate corresponding z
z = (-unit_vector[0] * xx - unit_vector[1] * yy - d) * 1. /unit_vector[2]
if(bPlot):
# plot the surface
fig = plt.figure()
ax = fig.add_subplot(111,projection='3d')
ax.plot_surface(xx, yy, z, alpha=0.2)
ax.scatter(p1.x, p1.y, p1.z, color='green')
ax.scatter(p2.x, p2.y, p2.z, color='green')
ax.scatter(p3.x, p3.y, p3.z, color='green')
ax.scatter(p_new.x, p_new.y, p_new.z, color='red')
#plt.show()
return p_error