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