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