geom.py
Go to the documentation of this file.
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


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04