def plot_image(self, P, axs, *args, **kwargs): ''' Method to project the 3D points into image and plot the image coordinates on a grid of dims (width, height) Parameters ---------- P : 3XN 3D points with respect to world ax : axes object of the plot Returns ------- p_all : 2D images coordinates of size num_cams X 2 X N masks : a boolean array of dimension num_cams X 1 X N indicating valid projects ''' tmp = helper_functions.compose_T(self.R, self.t.T)[:-1, :] P = helper_functions.euclid_to_homo(P) P_local = tmp @ P N = P.shape[1] masks = np.zeros((self.num_cams, 1, N)) p_all = np.zeros((self.num_cams, 2, N)) for i, c in enumerate(self.cams): axs[0, i].set_title('Axis [0, ' + str(i) + ']') p_all[i], masks[i] = c.plot_image(P_local, axs[0, i], *args, **kwargs) return p_all, masks
def plot_camera(self, ax, *args, **kwargs): ''' Method to plot the generalized camera array Parameters ---------- ax : axis object which should have been initialized before this call Returns ------- None. ''' # get transforms of camera array T_array = np.zeros((0, 4, 4)) for c in self.cams: T_cam = helper_functions.compose_T(c.poseR, c.poset.T) T_array = np.append(T_array, T_cam[None], axis=0) # Plot camera array at pos 1 wTc = helper_functions.compose_T(self.poseR, self.poset.T) helper_functions.plot_cam_array(ax, T_array, wTc, *args, **kwargs)
def transform(self, rot, trans): ''' Method to transform the current camera to a new pose Parameters ---------- rot : Rotation matrix 3x3 trans : translation vbector 3 x 1 Returns ------- None. ''' self.poset = self.poseR @ trans + self.poset self.poseR = self.poseR @ rot self.R = self.poseR.transpose() self.t = -1 * self.R @ self.poset tmp = helper_functions.compose_T(self.R, self.t.transpose())[:-1, :] self.P = self.K @ tmp
def __init__(self, focal=6 * 1e-3, resolution=(640, 480), pp=(320, 240), pix_siz=(5 * 1e-6, 5 * 1e-6), rot=np.eye(3), trans=np.zeros((3, 1)), noise=0): self.f = focal self.res = np.array([resolution[0], resolution[1]]) self.cx = pp[0] self.cy = pp[1] self.pixel_size = np.array([pix_siz[0], pix_siz[1]]) self.poseR = rot self.poset = trans self.R = rot.transpose() self.t = -1 * self.R @ trans self.K = np.array([[self.f / self.pixel_size[0], 0, self.cx], [0, self.f / self.pixel_size[1], self.cy], [0, 0, 1]]) tmp = helper_functions.compose_T(self.R, self.t.transpose())[:-1, :] self.P = self.K @ tmp self.noise = noise
def project(self, P): ''' Method to projectr 3D world points to image plane of the current camera Parameters ---------- p : 3D world points 3 X N Returns ------- p_all : 2D images coordinates of size num_cams X 2 X N masks : a boolean array of dimension num_cams X 1 X N indicating valid projects ''' #convert the 3D point from world into the generalized camera coordinate frame tmp = helper_functions.compose_T(self.R, self.t.T)[:-1, :] P = helper_functions.euclid_to_homo(P) P_local = tmp @ P N = P.shape[1] masks = np.zeros((self.num_cams, 1, N)) p_all = np.zeros((self.num_cams, 2, N)) for i, c in enumerate(self.cams): p_all[i], masks[i] = c.project(P_local) return p_all, masks
# #decompose the essential matrix to get R and t corrs_img1 = points1[: , corrs_mask] corrs_img2 = points2[: , corrs_mask] E_cv, mask = cv2.findEssentialMat(corrs_img1.T,corrs_img2.T,c.K) ret_val, R_cv, t_cv, mask1 = cv2.recoverPose(E_cv,corrs_img1.T,corrs_img2.T ) print("essential cv Rotation CV translation CV \n") print(np.array2string(E_cv[0,:]) +" "+np.array2string(R_cv[0,:]) + " " + np.array2string(t_cv[0,:])) print(np.array2string(E_cv[1,:]) +" "+np.array2string(R_cv[1,:]) + " " + np.array2string(t_cv[1,:])) print(np.array2string(E_cv[2,:]) +" "+np.array2string(R_cv[2,:]) + " " + np.array2string(t_cv[2,:])) print("\n\n\n") # pass the correspondences into GEC solver and estimate R and T using the essential matrix algo # plot the estimated R and T T_cam_est = helper_functions.compose_T(R,t.T) T_cam_est = helper_functions.T_inv(T_cam_est) print("essential Rotation translation \n") print(np.array2string(E[0,:]) +" "+np.array2string(R[0,:]) + " " + np.array2string(t[0,:])) print(np.array2string(E[1,:]) +" "+np.array2string(R[1,:]) + " " + np.array2string(t[1,:])) print(np.array2string(E[2,:]) +" "+np.array2string(R[2,:]) + " " + np.array2string(t[2,:])) helper_functions.plot_cam_array(ax1, T_array, T_cam_est, color = 'red') ################################################################### # PLOT THE CAMERA ROTATION AND TRANSLATION in a camera array # ################################################################### fig3,ax3 = helper_functions.initialize_3d_plot(number=3, limits = np.array([[-10,10],[-10,10],[-10,10]]), view=[-30,-90])