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 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 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 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 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 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 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 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));
