00001 import numpy as np 00002 from tf import transformations 00003 00004 def to_hom(points): 00005 if len(points.shape) == 1: 00006 points_hom = np.ones(points.shape[0] + 1,) 00007 points_hom[:-1] = points 00008 else: 00009 points_hom = np.ones((points.shape[0], points.shape[1]+1)) 00010 points_hom[:,:-1] = points 00011 return points_hom 00012 00013 def from_hom(points): 00014 if len(points.shape) == 1: 00015 return points[:-1] / points[-1] 00016 else: 00017 return points[:,:-1] / np.tile(np.reshape(points[:,-1], (len(points), 1)), (1, points.shape[1]-1)) 00018 00019 def transform_points(points, transform): 00020 ''' 00021 Transform one or more D dimensional points by the given homogeneous transform. 00022 00023 points - (D,) or (N, D) array 00024 00025 transform - (D + 1, D + 1) array 00026 ''' 00027 return from_hom(np.dot(to_hom(points), np.transpose(np.array(transform)))) 00028 00029 def transform_vectors(vectors, transform): 00030 R = np.array(transform[:3,:3]) / transform[3,3] 00031 return np.dot(vectors, np.transpose(R)) 00032 00033 def crop_to_bbox_mask(points, xmin, xmax, ymin, ymax, zmin, zmax): 00034 bbox_mask = (points[:,0] > xmin) & (points[:,0] < xmax) 00035 bbox_mask &= (points[:,1] > ymin) & (points[:,1] < ymax) 00036 bbox_mask &= (points[:,2] > zmin) & (points[:,2] < zmax) 00037 return bbox_mask 00038 00039 def crop_to_bbox(points, xmin, xmax, ymin, ymax, zmin, zmax): 00040 return points[crop_to_bbox_mask(points, xmin, xmax, ymin, ymax, zmin, zmax)] 00041 00042 def matrix_to_rot_trans(transform): 00043 ''' 00044 Converts a 4x4 homogenous rigid transformation matrix to a translation and a 00045 quaternion rotation. 00046 ''' 00047 scale, shear, angles, trans, persp = transformations.decompose_matrix(transform) 00048 rot = transformations.quaternion_from_euler(*angles) 00049 return rot, trans 00050 00051 def rot_trans_to_matrix(rot, trans): 00052 '''Converts a rotation and translation to a homogeneous transform. 00053 00054 Args: 00055 rot (np.array): Quaternion (x, y, z, w). 00056 trans (np.array): Translation (x, y, z). 00057 00058 Returns: 00059 H (np.array): 4x4 homogenous transform matrix. 00060 ''' 00061 H = transformations.quaternion_matrix(rot) 00062 H[0:3, 3] = trans 00063 return H