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 fit_sphere(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(30.0)):
'''
fit a sphere to the two point/normal pairs
eps: distance thres for determining if the fit is a valid sphere
both points must be < eps from the sphere
alpha: angle thres for determining if the fit is a valid sphere
both normals must be < alpha from the sphere normals
return (valid, center, radius);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1);
n1 = np.asarray(n1);
p2 = np.asarray(p2);
n2 = np.asarray(n2);
# find closes points on the lines
(pc0, pc1) = mu.line_line_closest_point(p1, n1, p2, n2);
# center of the sphere
c = (pc0 + pc1) / 2.0;
# compute radius
r = (np.linalg.norm(p1-c) + np.linalg.norm(p2-c))/2.0;
# check if the fit is valid
if ((np.abs(r - np.linalg.norm(p1 - c)) > eps)
or (np.abs(r - np.linalg.norm(p2 - c)) > eps)):
return (False, c, r);
sa = np.sin(alpha);
if ((np.sin(np.abs(ru.angle_between(n1, p1-c))) > sa)
or (np.sin(np.abs(ru.angle_between(n2, p2-c))) > sa)):
return (False, c, r);
return (True, c, r);
def fit_sphere(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(30.0)):
'''
fit a sphere to the two point/normal pairs
eps: distance thres for determining if the fit is a valid sphere
both points must be < eps from the sphere
alpha: angle thres for determining if the fit is a valid sphere
both normals must be < alpha from the sphere normals
return (valid, center, radius);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1)
n1 = np.asarray(n1)
p2 = np.asarray(p2)
n2 = np.asarray(n2)
# find closes points on the lines
(pc0, pc1) = mu.line_line_closest_point(p1, n1, p2, n2)
# center of the sphere
c = (pc0 + pc1) / 2.0
# compute radius
r = (np.linalg.norm(p1 - c) + np.linalg.norm(p2 - c)) / 2.0
# check if the fit is valid
if ((np.abs(r - np.linalg.norm(p1 - c)) > eps)
or (np.abs(r - np.linalg.norm(p2 - c)) > eps)):
return (False, c, r)
sa = np.sin(alpha)
if ((np.sin(np.abs(ru.angle_between(n1, p1 - c))) > sa)
or (np.sin(np.abs(ru.angle_between(n2, p2 - c))) > sa)):
return (False, c, r)
return (True, c, r)
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 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 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 get_centroid(pts):
'''
computes the centroid of the point cloud
'''
pts = ru.to_array(pts)
return np.mean(pts, axis=0)
def fit_plane(p1, n1, p2, n2, p3, n3, alpha=np.deg2rad(30.0)):
'''
fit a plane to the three point/normal pairs
alpha: angle thres for determining if the fit is a valid sphere
both normals must be < alpha from the sphere normals
return (valid, p, normal);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1);
n1 = np.asarray(n1);
p2 = np.asarray(p2);
n2 = np.asarray(n2);
p3 = np.asarray(p3);
n3 = np.asarray(n3);
v1 = p2 - p1;
v2 = p3 - p1;
n = np.cross(v1, v2);
n /= np.linalg.norm(n);
sa = np.sin(alpha);
sang = map(lambda nx: np.sin(np.abs(ru.angle_between(n, nx))), [n1, n2, n3]);
if (np.any(sang > sa)):
return (False, p1, n);
return (True, p1, n);
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 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 fit_plane(p1, n1, p2, n2, p3, n3, alpha=np.deg2rad(30.0)):
'''
fit a plane to the three point/normal pairs
alpha: angle thres for determining if the fit is a valid sphere
both normals must be < alpha from the sphere normals
return (valid, p, normal);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1)
n1 = np.asarray(n1)
p2 = np.asarray(p2)
n2 = np.asarray(n2)
p3 = np.asarray(p3)
n3 = np.asarray(n3)
v1 = p2 - p1
v2 = p3 - p1
n = np.cross(v1, v2)
n /= np.linalg.norm(n)
sa = np.sin(alpha)
sang = map(lambda nx: np.sin(np.abs(ru.angle_between(n, nx))),
[n1, n2, n3])
if (np.any(sang > sa)):
return (False, p1, n)
return (True, p1, n)
def get_centroid(pts):
'''
computes the centroid of the point cloud
'''
pts = ru.to_array(pts);
return np.mean(pts, axis=0);
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 get_chessboard_pose(img, info, ncol=5, nrow=4, squareSize=0.045, useSubPix=True, publishDetection=True, isRect=True):
'''
compute the pose of the chessboard in the camera frame
'''
(success, chessboard_wrtImage) = find_chessboard(img, ncol=ncol, nrow=nrow, useSubPix=useSubPix);
if (len(chessboard_wrtImage) == 0):
rospy.logwarn(__name__ + ': no chessboard found');
return None;
if (publishDetection):
chessboardCV = draw_chessboard(img, chessboard_wrtImage, ncol=ncol, nrow=nrow);
chessboardMsg = cvmat2msg(chessboardCV, img.encoding, frame_id=img.header.frame_id, stamp=rospy.Time().now());
chessboardDetectionPub.publish(chessboardMsg);
# compute pose
chessboard_wrtBoard = cv.CreateMat(ncol*nrow, 1, cv.CV_32FC3);
for i in range(ncol*nrow):
chessboard_wrtBoard[i,0] = ((i % ncol) * squareSize, (i / ncol) * squareSize, 0);
# get camera intrinsics
(cameraMatrix, distCoeffs, projectionMatrix, rotationMatrix) = info2cvmat(info);
# extrinsic outputs
rot = cv.CreateMat(3, 1, cv.CV_32FC1)
trans = cv.CreateMat(3, 1, cv.CV_32FC1)
# find chessboard pose
if isRect:
distCoeffs = cv.CreateMat(1, 4, cv.CV_32FC1);
distCoeffs[0,0] = 0; distCoeffs[0,1] = 0; distCoeffs[0,2] = 0; distCoeffs[0,3] = 0;
cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, projectionMatrix[:,:3], distCoeffs, rot, trans);
else:
cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, cameraMatrix, distCoeffs, rot, trans);
# get transform from rot, trans
rot = np.asarray(rot);
trans = np.asarray(trans);
th = np.linalg.norm(rot);
r = rot / th;
R = np.cos(th)*np.eye(3) + (1 - np.cos(th))*np.dot(r, r.T) + np.sin(th)*np.array([[0, -r[2], r[1]], [r[2], 0, -r[0]], [-r[1], r[0], 0]]);
if not isRect:
Trect = tr.trans(-projectionMatrix[0,3]/cameraMatrix[0,0],-projectionMatrix[1,3]/cameraMatrix[1,1], 0)
Rcam = tr.rot2tr(np.asarray(rotationMatrix));
Tmodel2cam = np.dot(Trect, np.dot(Rcam, np.dot(tr.trans(trans), tr.rot2tr(R))));
else:
Tmodel2cam = np.dot(tr.trans(trans), tr.rot2tr(R));
return ru.to_PoseStamped(Tmodel2cam, frame_id=info.header.frame_id, stamp=info.header.stamp);
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 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_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 Tdist(T0, T1, transWeight=1.0, rotWeight=1.0, method=2):
if (method == 1):
# 1 norm
td = np.linalg.norm(T0[:3,3] - T1[:3,3])
rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1), ord=1);
return (transWeight * td + rotWeight * rd);
if (method == 2):
# 2 norm
td = np.linalg.norm(T0[:3,3] - T1[:3,3])
rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1));
return (transWeight * td + rotWeight * rd);
if (method == 'ax'):
# squared angle between each axis
td = np.linalg.norm(T0[:3,3] - T1[:3,3])
(x0, y0, z0) = rot2ev(T0);
(x1, y1, z1) = rot2ev(T1);
rd = np.linalg.norm([ru.angle_between(v0, v1) for (v0, v1) in ((x0, x1), (y0, y1), (z0, z1))]);
return (transWeight * td + rotWeight * rd);
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 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 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 Tdist(T0, T1, transWeight=1.0, rotWeight=1.0, method=2):
if (method == 1):
# 1 norm
td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1), ord=1)
return (transWeight * td + rotWeight * rd)
if (method == 2):
# 2 norm
td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
rd = np.linalg.norm(rot2quat(T0) - rot2quat(T1))
return (transWeight * td + rotWeight * rd)
if (method == 'ax'):
# squared angle between each axis
td = np.linalg.norm(T0[:3, 3] - T1[:3, 3])
(x0, y0, z0) = rot2ev(T0)
(x1, y1, z1) = rot2ev(T1)
rd = np.linalg.norm([
ru.angle_between(v0, v1)
for (v0, v1) in ((x0, x1), (y0, y1), (z0, z1))
])
return (transWeight * td + rotWeight * rd)
def compute_pose(c, n):
v = np.cross([1, 0, 0], n)
v /= np.linalg.norm(v)
th = ru.angle_between([1, 0, 0], n)
return geometry_msgs.Pose(geometry_msgs.Point(*c),
tr.axis_angle2rosq(v, th))
def compute_pose(c, n): v = np.cross([1, 0, 0], n); v /= np.linalg.norm(v); th = ru.angle_between([1, 0, 0], n); return geometry_msgs.Pose(geometry_msgs.Point(*c), tr.axis_angle2rosq(v, th));
def fit_cone(p1, n1, p2, n2, p3, n3, eps=0.005, alpha=np.deg2rad(10.0)):
'''
fit a cone to the three point/normal pairs
eps: distance thres for determining if the fit is a valid cylinder
both points must be < eps from the cylinder
alpha: angle thres for determining if the fit is a valid cylinder
both normals must be < alpha from the cylinder normals
return (valid, apex, axis, opening angle);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1);
n1 = np.asarray(n1);
p2 = np.asarray(p2);
n2 = np.asarray(n2);
p3 = np.asarray(p3);
n3 = np.asarray(n3);
# find cone apex (intersection of the 3 planes formed by the point/normal pairs)
# line normal from plane 1/2 intersection
(lp, ln) = mu.plane_plane_intersection(p1, n1, p2, n2);
c = mu.line_plane_intersection(lp, ln, p3, n3);
# find the axis from the normal of the plane formed by the three points
pc1 = c + (p1 - c)/np.linalg.norm(p1 - c);
pc2 = c + (p2 - c)/np.linalg.norm(p2 - c);
pc3 = c + (p3 - c)/np.linalg.norm(p3 - c);
a = np.cross(pc2 - pc1, pc3 - pc1);
a /= np.linalg.norm(a);
# find opening anlge
ac = np.array(map(lambda p: ru.angle_between(p - c, a), [p1, p2, p3]));
w = np.sum(ac)/3.0;
# check validity
# project each point onto the axis
p1_proj_a = np.dot(p1 - c, a) * a;
p2_proj_a = np.dot(p2 - c, a) * a;
p3_proj_a = np.dot(p3 - c, a) * a;
# and rejections
p1_rej_a = (p1 - c) - p1_proj_a;
p2_rej_a = (p2 - c) - p2_proj_a;
p3_rej_a = (p3 - c) - p3_proj_a;
# projection mag
d1 = np.linalg.norm(p1_proj_a);
d2 = np.linalg.norm(p2_proj_a);
d3 = np.linalg.norm(p3_proj_a);
# mag of vector from axis to cone edge
r1 = d1 * np.tan(w);
r2 = d2 * np.tan(w);
r3 = d3 * np.tan(w);
# scale rejections to find the cone point
c1 = c + p1_proj_a + (r1/np.linalg.norm(p1_rej_a))*p1_rej_a;
c2 = c + p2_proj_a + (r2/np.linalg.norm(p2_rej_a))*p2_rej_a;
c3 = c + p3_proj_a + (r3/np.linalg.norm(p3_rej_a))*p3_rej_a;
# is the point within distance thres?
distToCone = np.array([np.linalg.norm(p1 - c1), np.linalg.norm(p2 - c2), np.linalg.norm(p3 - c3)]);
if np.any(distToCone > eps):
return (False, c, a, w);
# compute cone normals
cn1 = np.cross(np.cross(a, (c1 - c)), (c1 - c));
cn2 = np.cross(np.cross(a, (c2 - c)), (c2 - c));
cn3 = np.cross(np.cross(a, (c3 - c)), (c3 - c));
cn1 /= np.linalg.norm(cn1);
cn2 /= np.linalg.norm(cn2);
cn3 /= np.linalg.norm(cn3);
# are the normals close?
sa = np.sin(alpha);
if ((np.sin(np.abs(ru.angle_between(cn1, n1))) > sa)
or (np.sin(np.abs(ru.angle_between(cn2, n2))) > sa)
or (np.sin(np.abs(ru.angle_between(cn3, n3))) > sa)):
return (False, c, a, w);
return (True, c, a, w);
def fit_cylinder(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(10.0)):
'''
fit a cylinder to the two point/normal pairs
eps: distance thres for determining if the fit is a valid cylinder
both points must be < eps from the cylinder
alpha: angle thres for determining if the fit is a valid cylinder
both normals must be < alpha from the cylinder normals
return (valid, center, axis, radius);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1)
n1 = np.asarray(n1)
p2 = np.asarray(p2)
n2 = np.asarray(n2)
# find cylinder axis
a = np.cross(n1, n2)
a /= np.linalg.norm(a)
# project lines (defined by the point/normal) onto the plane defined by a.x = 0
pp1 = p1 - a * np.dot(a, p1)
pn1 = n1 - a * np.dot(a, n1)
pp2 = p2 - a * np.dot(a, p2)
pn2 = n2 - a * np.dot(a, n2)
# find intersection
(c, c1) = mu.line_line_closest_point(pp1, pn1, pp2, pn2)
# cylinder radius
r = np.linalg.norm(pp1 - c)
# check if the fit is valid
# find rejections of the points from the cylinder axis
cp1 = pp1 - c
cp2 = pp2 - c
ca = c + a
rej1 = cp1 - np.dot(cp1, ca) * ca
rej2 = cp2 - np.dot(cp2, ca) * ca
pa = create_normals_pose_array(np.array([p1, p2]), np.array([rej1, rej2]))
#Debug.pa1Pub.publish(pa);
print rej1
print rej2
print np.sin(np.abs(ru.angle_between(rej1, n1)))
print np.sin(np.abs(ru.angle_between(rej2, n2)))
print np.abs(r - np.linalg.norm(rej1))
print np.abs(r - np.linalg.norm(rej2))
sa = np.sin(alpha)
if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
return (False, c, a, r)
if ((np.abs(r - np.linalg.norm(rej1)) > eps)
or (np.abs(r - np.linalg.norm(rej2)) > eps)):
return (False, c, a, r)
sa = np.sin(alpha)
if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
return (False, c, a, r)
return (True, c, a, r)
def fit_cone(p1, n1, p2, n2, p3, n3, eps=0.005, alpha=np.deg2rad(10.0)):
'''
fit a cone to the three point/normal pairs
eps: distance thres for determining if the fit is a valid cylinder
both points must be < eps from the cylinder
alpha: angle thres for determining if the fit is a valid cylinder
both normals must be < alpha from the cylinder normals
return (valid, apex, axis, opening angle);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1)
n1 = np.asarray(n1)
p2 = np.asarray(p2)
n2 = np.asarray(n2)
p3 = np.asarray(p3)
n3 = np.asarray(n3)
# find cone apex (intersection of the 3 planes formed by the point/normal pairs)
# line normal from plane 1/2 intersection
(lp, ln) = mu.plane_plane_intersection(p1, n1, p2, n2)
c = mu.line_plane_intersection(lp, ln, p3, n3)
# find the axis from the normal of the plane formed by the three points
pc1 = c + (p1 - c) / np.linalg.norm(p1 - c)
pc2 = c + (p2 - c) / np.linalg.norm(p2 - c)
pc3 = c + (p3 - c) / np.linalg.norm(p3 - c)
a = np.cross(pc2 - pc1, pc3 - pc1)
a /= np.linalg.norm(a)
# find opening anlge
ac = np.array(map(lambda p: ru.angle_between(p - c, a), [p1, p2, p3]))
w = np.sum(ac) / 3.0
# check validity
# project each point onto the axis
p1_proj_a = np.dot(p1 - c, a) * a
p2_proj_a = np.dot(p2 - c, a) * a
p3_proj_a = np.dot(p3 - c, a) * a
# and rejections
p1_rej_a = (p1 - c) - p1_proj_a
p2_rej_a = (p2 - c) - p2_proj_a
p3_rej_a = (p3 - c) - p3_proj_a
# projection mag
d1 = np.linalg.norm(p1_proj_a)
d2 = np.linalg.norm(p2_proj_a)
d3 = np.linalg.norm(p3_proj_a)
# mag of vector from axis to cone edge
r1 = d1 * np.tan(w)
r2 = d2 * np.tan(w)
r3 = d3 * np.tan(w)
# scale rejections to find the cone point
c1 = c + p1_proj_a + (r1 / np.linalg.norm(p1_rej_a)) * p1_rej_a
c2 = c + p2_proj_a + (r2 / np.linalg.norm(p2_rej_a)) * p2_rej_a
c3 = c + p3_proj_a + (r3 / np.linalg.norm(p3_rej_a)) * p3_rej_a
# is the point within distance thres?
distToCone = np.array([
np.linalg.norm(p1 - c1),
np.linalg.norm(p2 - c2),
np.linalg.norm(p3 - c3)
])
if np.any(distToCone > eps):
return (False, c, a, w)
# compute cone normals
cn1 = np.cross(np.cross(a, (c1 - c)), (c1 - c))
cn2 = np.cross(np.cross(a, (c2 - c)), (c2 - c))
cn3 = np.cross(np.cross(a, (c3 - c)), (c3 - c))
cn1 /= np.linalg.norm(cn1)
cn2 /= np.linalg.norm(cn2)
cn3 /= np.linalg.norm(cn3)
# are the normals close?
sa = np.sin(alpha)
if ((np.sin(np.abs(ru.angle_between(cn1, n1))) > sa)
or (np.sin(np.abs(ru.angle_between(cn2, n2))) > sa)
or (np.sin(np.abs(ru.angle_between(cn3, n3))) > sa)):
return (False, c, a, w)
return (True, c, a, w)
def fit_cylinder(p1, n1, p2, n2, eps=0.005, alpha=np.deg2rad(10.0)):
'''
fit a cylinder to the two point/normal pairs
eps: distance thres for determining if the fit is a valid cylinder
both points must be < eps from the cylinder
alpha: angle thres for determining if the fit is a valid cylinder
both normals must be < alpha from the cylinder normals
return (valid, center, axis, radius);
valid is a bool indicating if the fit was valid based on the points and normals
'''
p1 = np.asarray(p1);
n1 = np.asarray(n1);
p2 = np.asarray(p2);
n2 = np.asarray(n2);
# find cylinder axis
a = np.cross(n1, n2);
a /= np.linalg.norm(a);
# project lines (defined by the point/normal) onto the plane defined by a.x = 0
pp1 = p1 - a * np.dot(a, p1);
pn1 = n1 - a * np.dot(a, n1);
pp2 = p2 - a * np.dot(a, p2);
pn2 = n2 - a * np.dot(a, n2);
# find intersection
(c, c1) = mu.line_line_closest_point(pp1, pn1, pp2, pn2);
# cylinder radius
r = np.linalg.norm(pp1 - c);
# check if the fit is valid
# find rejections of the points from the cylinder axis
cp1 = pp1 - c;
cp2 = pp2 - c;
ca = c + a;
rej1 = cp1 - np.dot(cp1, ca)*ca;
rej2 = cp2 - np.dot(cp2, ca)*ca;
pa = create_normals_pose_array(np.array([p1,p2]), np.array([rej1,rej2]));
#Debug.pa1Pub.publish(pa);
print rej1
print rej2
print np.sin(np.abs(ru.angle_between(rej1, n1)))
print np.sin(np.abs(ru.angle_between(rej2, n2)))
print np.abs(r - np.linalg.norm(rej1))
print np.abs(r - np.linalg.norm(rej2))
sa = np.sin(alpha);
if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
return (False, c, a, r);
if ((np.abs(r - np.linalg.norm(rej1)) > eps)
or (np.abs(r - np.linalg.norm(rej2)) > eps)):
return (False, c, a, r);
sa = np.sin(alpha);
if (((np.sin(np.abs(ru.angle_between(rej1, n1)))) > sa)
or (np.sin(np.abs(ru.angle_between(rej2, n2))) > sa)):
return (False, c, a, r);
return (True, c, a, r);