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)
