def homogenous_transform_from_dict(d):
"""
Returns a transform from a standard encoding in dict format
:param d:
:return:
"""
pos = [0] * 3
pos[0] = d['translation']['x']
pos[1] = d['translation']['y']
pos[2] = d['translation']['z']
quatDict = getQuaternionFromDict(d)
quat = [0] * 4
quat[0] = quatDict['w']
quat[1] = quatDict['x']
quat[2] = quatDict['y']
quat[3] = quatDict['z']
transform_matrix = transformations.quaternion_matrix(quat)
transform_matrix[0:3, 3] = np.array(pos)
return transform_matrix
def updateData(self, new_pc2):
pc = ros_numpy.numpify(new_pc2)
h = pc.shape[0]
w = pc.shape[1]
points = np.zeros([h, w, 3])
points[:, :, 0] = pc['x']
points[:, :, 1] = pc['y']
points[:, :, 2] = pc['z']
good_mask = (np.isfinite(points[:, :, 2]) + 1.0) / 2.0
split_rgb_pc = ros_numpy.point_cloud2.split_rgb_field(pc)
self.current_rgb_image = np.zeros([h, w, 3], dtype=np.int8)
self.current_rgb_image[:, :, 0] = split_rgb_pc['r'] * good_mask
self.current_rgb_image[:, :, 1] = split_rgb_pc['g'] * good_mask
self.current_rgb_image[:, :, 2] = split_rgb_pc['b'] * good_mask
qimage = QtGui.QImage(self.current_rgb_image,
self.current_rgb_image.shape[1],
self.current_rgb_image.shape[0],
self.current_rgb_image.shape[1] * 3,
QtGui.QImage.Format_RGB888)
self.viewer_pixmap = QtGui.QPixmap(qimage)
self.viewer.setImage(self.viewer_pixmap)
self.current_depth_image = points[:, :, 2]
try:
(trans,
rot) = self.tf_listener.lookupTransform(self.base_frame,
self.rgb_camera_frame,
rospy.Time(0))
self.current_camera_pose = transformations.quaternion_matrix(
np.array([rot[3], rot[0], rot[1], rot[2]]))
self.current_camera_pose[:3, 3] = trans[:]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
self.current_camera_pose = np.eye(4)
rospy.logwarn("Couldn't get TF from %s to %s" %
(self.base_frame, self.rgb_camera_frame))
def tf_matrix_from_pose(pose):
trans, quat = pose
mat = transformations.quaternion_matrix(quat)
mat[:3, 3] = trans
return mat