def set_extrinsics(self,campos,upvec,camtar=None,view_dir=None,opt_axis = False):
if camtar is not None:
w = np.squeeze(np.array(camtar) - np.array(campos))
elif view_dir is not None:
w = view_dir
else:
raise ValueError('Either viewing target or view direction must be specified!')
w = w / np.sqrt(np.sum(w**2))
v = upvec / np.sqrt(np.sum(upvec**2))
# opt_axis specifies whether the input campos or camtar are where we should
# point the optical axis or the view centre. By defauly we assume the view centre.
# In this case, we need another rotation matrix to rotate from the image centre
# view direction (given) to the optical axis direction (stored). This applies for
# intrinsics where the perspective centre is not at the the detector centre.
if not opt_axis:
R = np.zeros([3,3])
# Optical axis direction in the camera coordinate system
uz = np.array(self.normalise(self.image_display_shape[0]/2.,self.image_display_shape[1]/2.,0) + (1.,))
uz = uz / np.sqrt(np.sum(uz**2))
ux = np.array([1,0,0]) - uz[0]*uz
ux = ux / np.sqrt(np.sum(ux**2))
uy = np.cross(ux,uz)
R[:,0] = ux
R[:,1] = -uy
R[:,2] = uz
R = np.matrix(R).T
else:
# If we are pointing the optical axis, set this extra
# rotation to be the identity.
R = np.matrix([[1,0,0],[0,1,0],[0,0,1]])
u = np.cross(w,v)
Rmatrix = np.zeros([3,3])
Rmatrix[:,0] = u
Rmatrix[:,1] = -v
Rmatrix[:,2] = w
Rmatrix = np.matrix(Rmatrix)
Rmatrix = Rmatrix * R
campos = np.matrix(campos)
if campos.shape[0] < campos.shape[1]:
campos = campos.T
self.fit_params[0].tvec = -Rmatrix.T * campos
self.fit_params[0].rvec = -cv2.Rodrigues(Rmatrix)[0]
评论列表
文章目录
