def order_transforms(Tlist, Tbase, transWeight=1.0, rotWeight=1.0, method=2): ''' orders the Transforms/Frames ''' Tlist = [ru.to_array(T) for T in Tlist]; Tbase = ru.to_array(Tbase); d = [Tdist(T, Tbase, transWeight, rotWeight, method) for T in Tlist]; return (np.argsort(d), d);
def order_transforms(Tlist, Tbase, transWeight=1.0, rotWeight=1.0, method=2): ''' orders the Transforms/Frames ''' Tlist = [ru.to_array(T) for T in Tlist] Tbase = ru.to_array(Tbase) d = [Tdist(T, Tbase, transWeight, rotWeight, method) for T in Tlist] return (np.argsort(d), d)
def get_centroid(pts): ''' computes the centroid of the point cloud ''' pts = ru.to_array(pts); return np.mean(pts, axis=0);
def estimate_normals(pts, k=5, weightNeighbors=True, viewpoint=None): ''' estimates the point cloud normals from an unorder point cloud pts: NxD point array k: number of nearest neighbors to consider weightNeighbors: flag indicating that weights on the neighbors should be used viewpoint: vector indicating the viewpoint direction to correct direction ''' def compute_normal(pts): cent = np.mean(pts) cov = np.cov(np.transpose(pts - cent)) (u, s, vh) = np.linalg.svd(cov) return vh[2, :] pts = ru.to_array(pts) # compute distance matrix d = distance_matrix(pts, pts) # find nearest neighbors nn = np.argsort(d, axis=1) normals = np.array(map(lambda row: compute_normal(pts[row[:k + 1], :]), nn)) if (viewpoint != None): vcheck = map(lambda i: np.dot(normals[i, :], viewpoint - pts[i, :]), np.arange(pts.shape[0])) normals[(vcheck < 0), :] *= -1.0 return normals
def get_centroid(pts): ''' computes the centroid of the point cloud ''' pts = ru.to_array(pts) return np.mean(pts, axis=0)
def get_bbox(pc, pose=None): ''' get the bounding box for the point cloud if a pose is provided then it will find the bbox for the data aligned to that pose return: bbox array |xmin, xmax| |ymin, ymax| |zmin, zmax| ''' pts = ru.to_array(pc); if pose != None: ps = ru.to_PoseStamped(pose); ps_wrtPC = ru.transformPose(ru.get_frame_id(pc), ps); T = tr.rospose2tr(ps_wrtPC.pose); pts = np.dot(np.linalg.inv(T), np.vstack((pts.T, np.ones(pts.shape[0])))).T; pmin = np.min(pts[:,:3], axis=0); pmax = np.max(pts[:,:3], axis=0); bbox = np.array([pmin, pmax]).T; return bbox;
def get_bbox(pc, pose=None): ''' get the bounding box for the point cloud if a pose is provided then it will find the bbox for the data aligned to that pose return: bbox array |xmin, xmax| |ymin, ymax| |zmin, zmax| ''' pts = ru.to_array(pc) if pose != None: ps = ru.to_PoseStamped(pose) ps_wrtPC = ru.transformPose(ru.get_frame_id(pc), ps) T = tr.rospose2tr(ps_wrtPC.pose) pts = np.dot(np.linalg.inv(T), np.vstack( (pts.T, np.ones(pts.shape[0])))).T pmin = np.min(pts[:, :3], axis=0) pmax = np.max(pts[:, :3], axis=0) bbox = np.array([pmin, pmax]).T return bbox
def ev2rot(*args, **kwargs): ''' converts eigen vectors to a 4x4 transform representing the rotation if eigen values are provided it will sort the vectors if no values are provided it is assumed the vectors are sorted accepted args: - iterable of 3 'vectors' - 3 vector arguments kwargs: - 'evals': iterable of 3 values returns the Quaternion as an array NOTE: this is not the ros pose convention [x, y, z, w]. use ev2rosq(r) to get a ros quaternion message ''' if (len(args) == 1): evec = np.array(map(ru.to_array, args[0])); if (len(args) > 1): evec = np.array(map(ru.to_array, args[:3])); if (kwargs.has_key('evals')): # sort based on eigen values (high->low) isortEvals = np.argsort(ru.to_array(kwargs['evals']))[::-1]; evec = evec[isortEvals]; # normalize evec = np.array([(ev / np.linalg.norm(ev)) for ev in evec]); R = np.eye(4); R[:3,:3] = evec.T; return R;
def ev2rot(*args, **kwargs): ''' converts eigen vectors to a 4x4 transform representing the rotation if eigen values are provided it will sort the vectors if no values are provided it is assumed the vectors are sorted accepted args: - iterable of 3 'vectors' - 3 vector arguments kwargs: - 'evals': iterable of 3 values returns the Quaternion as an array NOTE: this is not the ros pose convention [x, y, z, w]. use ev2rosq(r) to get a ros quaternion message ''' if (len(args) == 1): evec = np.array(map(ru.to_array, args[0])) if (len(args) > 1): evec = np.array(map(ru.to_array, args[:3])) if (kwargs.has_key('evals')): # sort based on eigen values (high->low) isortEvals = np.argsort(ru.to_array(kwargs['evals']))[::-1] evec = evec[isortEvals] # normalize evec = np.array([(ev / np.linalg.norm(ev)) for ev in evec]) R = np.eye(4) R[:3, :3] = evec.T return R
def estimate_normals(pts, k=5, weightNeighbors=True, viewpoint=None): ''' estimates the point cloud normals from an unorder point cloud pts: NxD point array k: number of nearest neighbors to consider weightNeighbors: flag indicating that weights on the neighbors should be used viewpoint: vector indicating the viewpoint direction to correct direction ''' def compute_normal(pts): cent = np.mean(pts); cov = np.cov(np.transpose(pts - cent)); (u, s, vh) = np.linalg.svd(cov); return vh[2,:]; pts = ru.to_array(pts); # compute distance matrix d = distance_matrix(pts, pts); # find nearest neighbors nn = np.argsort(d, axis=1); normals = np.array(map(lambda row: compute_normal(pts[row[:k+1],:]), nn)); if (viewpoint != None): vcheck = map(lambda i: np.dot(normals[i,:], viewpoint - pts[i,:]), np.arange(pts.shape[0])); normals[(vcheck < 0),:] *= -1.0; return normals;
def project_points(points, caminfo): ''' projects the 3D point into the desired image coordinates points: array of points to project (default is Nx3) in the camera frame return: Nx2 array of projected points ''' # format input correctly points = ru.to_array(points); pshape = points.shape; if (len(pshape) < 2): points = np.array([points]); elif (max(pshape) > 3): if (pshape[1] > pshape[0]): points = points.T; elif (max(pshape) == 3): if (pshape[0] > pshape[1]): points = points.T; # get camera matrix C = np.reshape(np.array(caminfo.K), (3,3)); # get distorition coef (k1, k2, p1, p2, k3) = caminfo.D; # normalize with z points = points.T/points[:,2]; # undistort r2 = np.sum(points[:2,:]**2, axis=0); xy = np.prod(points[:2,:], axis=0); d1 = (1.0 + k1*r2 + k2*r2**2 + k3*r2**3); c1 = points[:2,:]*np.array([d1,d1]); c2 = np.array([2.0*p1*xy, 2.0*p2*xy]); c3 = np.array([p2*(r2 + 2*points[0,:]**2), p1*(r2 + 2*points[1,:]**2)]); points[:2,:] = c1 + c2 + c3; return C; # project prj = np.dot(C, points); if (prj.shape[1] == 1): return prj[:2,0].T; else: return prj[:2,:].T;
def get_blob_pose(pts): ''' computes the pose of the point cloud arr is expected to be Nx3 return Pose ''' pts = ru.to_array(pts) cent = get_centroid(pts) (x, y, z) = get_ev(pts) pose = geometry_msgs.Pose() pose.position = geometry_msgs.Point(*cent) pose.orientation = tr.ev2rosq((x, y, z)) return pose
def get_blob_pose(pts): ''' computes the pose of the point cloud arr is expected to be Nx3 return Pose ''' pts = ru.to_array(pts); cent = get_centroid(pts); (x, y, z) = get_ev(pts); pose = geometry_msgs.Pose(); pose.position = geometry_msgs.Point(*cent); pose.orientation = tr.ev2rosq((x,y,z)); return pose;
def get_ev(pts, includeEigenValues=False): ''' computes the eigen vectors for the point cloud arr is expected to be Nx3 return: array of row vectors (e1, e2, e3) ''' pts = ru.to_array(pts) trans = -1 * np.mean(pts, axis=0) pts += trans cov = np.cov(pts.T) (u, s, vh) = np.linalg.svd(cov) if includeEigenValues: return (vh, s) else: return vh
def get_ev(pts, includeEigenValues=False): ''' computes the eigen vectors for the point cloud arr is expected to be Nx3 return: array of row vectors (e1, e2, e3) ''' pts = ru.to_array(pts); trans = -1 * np.mean(pts, axis=0); pts += trans; cov = np.cov(pts.T); (u, s, vh) = np.linalg.svd(cov); if includeEigenValues: return (vh, s); else: return vh;
def translate_rospose(pose, t): ''' translates a ros pose with respect to the poses orientation return will be a Pose or PoseStamped based on the input (header will be copied) ''' t = ru.to_array(t); if (t.ndim == 2): # 4x4 transform (extract the translation from it) t = trans(t[:3,3]); else: t = trans(t[:3]); T = rospose2tr(pose); nT = np.dot(T, t); if (type(pose) == geometry_msgs.PoseStamped): return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header)); else: return tr2rospose(nT);
def rotate_rospose(pose, r): ''' rotates a ros pose orientation with respect to the poses current orientation rot: either list of EulerZYX angles (z, y, x) or 4x4 transform matrix return will be a Pose or PoseStamped based on the input (header will be copied) ''' R = ru.to_array(r); if (R.ndim == 2): # 4x4 transform (extract the rotation from it) R[:3,3] = 0.0; else: R = ezyx(r); T = rospose2tr(pose); nT = np.dot(T, R); if (type(pose) == geometry_msgs.PoseStamped): return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header)); else: return tr2rospose(nT);
def translate_rospose(pose, t): ''' translates a ros pose with respect to the poses orientation return will be a Pose or PoseStamped based on the input (header will be copied) ''' t = ru.to_array(t) if (t.ndim == 2): # 4x4 transform (extract the translation from it) t = trans(t[:3, 3]) else: t = trans(t[:3]) T = rospose2tr(pose) nT = np.dot(T, t) if (type(pose) == geometry_msgs.PoseStamped): return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header)) else: return tr2rospose(nT)
def rotate_rospose(pose, r): ''' rotates a ros pose orientation with respect to the poses current orientation rot: either list of EulerZYX angles (z, y, x) or 4x4 transform matrix return will be a Pose or PoseStamped based on the input (header will be copied) ''' R = ru.to_array(r) if (R.ndim == 2): # 4x4 transform (extract the rotation from it) R[:3, 3] = 0.0 else: R = ezyx(r) T = rospose2tr(pose) nT = np.dot(T, R) if (type(pose) == geometry_msgs.PoseStamped): return geometry_msgs.PoseStamped(pose=tr2rospose(nT), header=copy.copy(pose.header)) else: return tr2rospose(nT)