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])