Find Rotation Matrix to align Basis vectors to world coordinate

36 Views Asked by At

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

enter image description here