#!/usr/bin/python import numpy.linalg import numpy import math, struct from mat3 import * origpoints = [ (-0.5, -0.5, 0.0), (-0.5, 0.5, 0.0), ( 0.5, 0.5, 0.0), ( 0.5, -0.5, 0.0), (-0.5, -0.5, 0.5), (-0.5, 0.5, 0.5), ] def sq(a,b): s = sum( [ (a[i]-b[i])**2 for i in range(len(a)) ] ) return math.sqrt(s) class Camera: def __init__(self, thepoints): self.thepoints = thepoints (X1,Y1,Z1),(X2,Y2,Z2),(X3,Y3,Z3),(X4,Y4,Z4),(X5,Y5,Z5),(X6,Y6,Z6) = origpoints[:6] (u1,v1),(u2,v2),(u3,v3),(u4,v4),(u5,v5),(u6,v6) = thepoints PE = numpy.array([ [ X1, Y1, Z1, 1, 0, 0, 0, 0, -u1*X1, -u1*Y1, -u1*Z1,], [ 0, 0, 0, 0, X1, Y1, Z1, 1, -v1*X1, -v1*Y1, -v1*Z1,], [ X2, Y2, Z2, 1, 0, 0, 0, 0, -u2*X2, -u2*Y2, -u2*Z2,], [ 0, 0, 0, 0, X2, Y2, Z2, 1, -v2*X2, -v2*Y2, -v2*Z2,], [ X3, Y3, Z3, 1, 0, 0, 0, 0, -u3*X3, -u3*Y3, -u3*Z3,], [ 0, 0, 0, 0, X3, Y3, Z3, 1, -v3*X3, -v3*Y3, -v3*Z3,], [ X4, Y4, Z4, 1, 0, 0, 0, 0, -u4*X4, -u4*Y4, -u4*Z4,], [ 0, 0, 0, 0, X4, Y4, Z4, 1, -v4*X4, -v4*Y4, -v4*Z4,], [ X5, Y5, Z5, 1, 0, 0, 0, 0, -u5*X5, -u5*Y5, -u5*Z5,], [ 0, 0, 0, 0, X5, Y5, Z5, 1, -v5*X5, -v5*Y5, -v5*Z5,], [ X6, Y6, Z6, 1, 0, 0, 0, 0, -u6*X6, -u6*Y6, -u6*Z6,], [ 0, 0, 0, 0, X6, Y6, Z6, 1, -v6*X6, -v6*Y6, -v6*Z6,], ]) S = numpy.array([u1, v1,u2, v2, u3, v3, u4 ,v4 ,u5 ,v5, u6, v6]).T v, _, _, _ = numpy.linalg.lstsq(PE, S) p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, = v self.A = numpy.array([ [p11, p12, p13, p14], [p21, p22, p23, p24], [p31, p32, p33, 1.0], ]) def plane_equation(self, U, V): A = self.A a1 = A[0,0] - U*A[2,0] b1 = A[0,1] - U*A[2,1] c1 = A[0,2] - U*A[2,2] d1 = A[0,3] - U*A[2,3] a2 = A[1,0] - V*A[2,0] b2 = A[1,1] - V*A[2,1] c2 = A[1,2] - V*A[2,2] d2 = A[1,3] - V*A[2,3] return a1,b1,c1,d1,a2,b2,c2,d2 class Cameras: cs = [] def __init__(self, thepoints_list): if len(thepoints_list[0][0]) == 3: thepoints_list = [map(lambda (x,y,_):(x,y), tp) for tp in thepoints_list] self.cs = [Camera(thepoints) for thepoints in thepoints_list] def count(self, points, dores=False): assert(len(points)<= len(self.cs)) bb = [] aa = [] for i,p in enumerate(points): a1,b1,c1,d1,a2,b2,c2,d2 = self.cs[i].plane_equation(*p) aa.extend([[a1,b1,c1],[a2,b2,c2]]) bb.extend([-d1,-d2]) v, res, _, _ = numpy.linalg.lstsq(numpy.array(aa), numpy.array(bb)) if dores: return (v[0], v[1], v[2]), res[0] return (v[0], v[1], v[2]) def test(self): for i in range(6): print "#%i" % i v = self.count([c.thepoints[i] for c in self.cs]) print "%6.3f %6.3f %6.3f sq=%f" % (v[0], v[1], v[2], sq(v, origpoints[i])) if __name__ == '__main__': thepoints1=[(0.5380859375, 0.81640625, 1), (0.8955078125, 0.88411458333333337, 3), (0.2392578125, 0.92317708333333337, 2), (0.0615234375, 0.82161458333333337, 1), (0.5478515625, 0.51432291666666663, 2), (0.8828125, 0.44140625, 2)] thepoints1 = map(lambda (x,y,_):(x,y), thepoints1) thepoints2=[(0.3125, 0.734375, 1), (0.7470703125, 0.72786458333333337, 1), (0.890625, 0.80208333333333337, 2), (0.23046875, 0.80598958333333337, 2), (0.3095703125, 0.44791666666666669, 1), (0.7353515625, 0.44010416666666669, 1)] thepoints2 = map(lambda (x,y,_):(x,y), thepoints2) c = Cameras([thepoints1,thepoints2]) c.test() thepoint=[0,0] thepoint[0]=[(0.2880859375, 0.75911458333333337, 1), (0.6923828125, 0.73828125, 1), (0.841796875, 0.796875, 2), (0.255859375, 0.82552083333333337, 2), (0.283203125, 0.49088541666666669, 1), (0.6806640625, 0.47916666666666669, 2)] thepoint[1]=[(0.515625, 0.78255208333333337, 1), (0.91796875, 0.83984375, 2), (0.392578125, 0.890625, 2), (0.0732421875, 0.79427083333333337, 2), (0.53125, 0.48828125, 1), (0.9248046875, 0.44010416666666669, 3)] c = Cameras(thepoint) c.test() thepoint[0]=[(0.2880859375, 0.75911458333333337, 1), (0.69140625, 0.73958333333333337, 1), (0.8388671875, 0.78645833333333337, 1), (0.2578125, 0.82421875, 2), (0.283203125, 0.49479166666666669, 1), (0.6796875, 0.47786458333333331, 2)] thepoint[1]=[(0.515625, 0.78125, 1), (0.91015625, 0.84244791666666663, 1), (0.3984375, 0.890625, 2), (0.0751953125, 0.79557291666666663, 2), (0.5302734375, 0.4921875, 1), (0.9111328125, 0.43619791666666669, 2)] #{'p2': (0.390625, 0.80729166666666663, 2), 'p1': (0.43359375, 0.77213541666666663, 1)} c = Cameras(thepoint) c.test() ''' [?,?,?,?] [?,?,?,?] [?,?,?,?] [0,0,0,1] 12 param -> 6 alpha, beta, gamma, x, y, z -> 6 X1 = [cosY*cosZ*x1 + -1*cosY*sinZ*y1 + sinY*z1] Y1 = [sinX*sinY*cosZ + cosX*sinZ*x1 + -sinX*sinY*sinZ + cosX*cosZ*y1 + -1*sinX*cosY*z1] Z1 = [-1*cosX*sinY*cosZ + sinX*sinZ*x1 + cosX*sinY*sinZ + sinX*cosZ*y1 + cosX*cosY*z1] X2 = [cosY*cosZ*x2 + -1*cosY*sinZ*y2 + sinY*z2] Y2 = [sinX*sinY*cosZ + cosX*sinZ*x2 + -sinX*sinY*sinZ + cosX*cosZ*y2 + -1*sinX*cosY*z2] Z2 = [-1*cosX*sinY*cosZ + sinX*sinZ*x2 + cosX*sinY*sinZ + sinX*cosZ*y2 + cosX*cosY*z2] ''' ''' class Move: start_points = [] cameras = [] def __init__(self, start_points, cameras): self.start_points = start_points self.cameras = cameras[:] def change(self, some_points): assert(len(some_points) == len(self.start_points)) def getdelta(self): ''' def change_order_list(l, order): return [l[idx] for idx in order] def change_order_lists(lists, order): return [ change_order_list(l, order) for l in lists ] def pair_points(points): # points from 1st camera ordered by height order = map(lambda (i,(x,y)):i, sorted(enumerate(points[0]), key=lambda (i,(x,y)):y, reverse=True)) points[0] = change_order_list(points[0], order) camera_first = points[0] # for every camera for camera_points in points[1:]: order = [] # count the d for (x,y) in camera_first: dists = dict( map(lambda (i,(a,b)):(((x-a)**2 + (y-b)**2),i), enumerate(points_one)) ) minkey = min(dists.keys()) minidx = dists[minkey] order.append( minidx ) points_one.remove( points_one[minidx] ) points = change_order_lists(points, order) order = map(lambda (i,(x,y)):i, sorted(enumerate(points[0]), key=lambda (i,(x,y)):y, reverse=True)) start_points = change_order_lists(points, order) def count_points(cs, cameras_reads): if len(cameras_reads[0]) == 1: return [cs.count( [camera_reads[0] for camera_reads in cameras_reads] )] if len(cameras_reads[0]) == 2: assert(len(cameras_reads) == 2) p1, pr1 = cs.count( [cameras_reads[0][0], cameras_reads[1][0]], dores=True ) p2, pr2 = cs.count( [cameras_reads[0][1], cameras_reads[1][1]], dores=True ) q1, qr1 = cs.count( [cameras_reads[0][0], cameras_reads[1][1]], dores=True ) q2, qr2 = cs.count( [cameras_reads[0][1], cameras_reads[1][0]], dores=True ) pd = pr1 + pr2 qd = qr1 + qr2 return [p1, p2] if pd < qd else [q1, q2] raise NotImplemented def sort_ps_by_z(ps): return sorted(ps, key=lambda (x,y,z):z, reverse=False) def sort_ps_by_pair(new_ps, old_ps): if len(old_ps) == 1 or len(new_ps) == 1: return new_ps def dist(p,q): if p == Ellipsis or q == Ellipsis: return Ellipsis x,y,z = p a,b,c = q return ((x-a)**2)+((y-b)**2)+((z-c)**2) order = [] nps = new_ps[:] for p in old_ps: dists = dict( map(lambda (i,q):(dist(p,q), i), enumerate(nps))) minkey = min(dists.keys()) minidx = dists[minkey] order.append( minidx ) nps[minidx] = Ellipsis return change_order_list(new_ps, order) def axis_angle_to_deg(axis, angle): return map(lambda a:a*math.degrees(angle),axis) counter = 0 #robot_position = [885 *1000, 0 *1000, 470 *1000, [0,0,0], 0] #uf1 robot_position = [843.0, 643.0, 143.0, [0,0,1], math.radians(-9) ] #base #robot_position = [815.0, -24.0, -474.0, [-0.99,0.0,-0.01], math.radians(179) ] delta_position = [0,0,0,[1.0,0,0],0] def update_robot_position( (xn,yn,zn, axisn, anglen), commit=False): global robot_position, counter x,y,z,axis,angle = robot_position ''' A = numpy.array([ [1,0,0,x+xn], [0,1,0,y+yn], [0,0,1,z+zn], [0,0,0,1], ]) ''' aa = axis_angle_to_deg(axis, angle) bb = axis_angle_to_deg(axisn, anglen) cc = [ aa[i]+bb[i] for i in range(len(aa)) ] # in degrees cc = map(math.radians, cc) # in radians axis, angle = unitize(cc) D = numpy.array( rotation_axis_to_matrix(axis, angle) ) # go through the matrix axis, angle = rotation_matrix_to_axis( D ) # x,y,z = x+xn, y+yn, z+zn #axis, m = unitize(axis) #angle = angle * m a,b,g = axis_angle_to_deg(axis, angle) an,bn,gn = axis_angle_to_deg(axisn, anglen) print "\b"*150, counter += 1 c = r'/-\|' print "final=(%6i %6i %6i || %3i %3i %3i) move=(%6i %6i %6i || %3i %3i %3i) %s" % ( x,y,z,a,b,g, xn,yn,zn,an,bn,gn, c[counter % len(c)]), update_robot( (x,y,z,axis, angle) ) if commit: robot_position = [x,y,z,axis,angle] print "commmit" old_ps = None mid_ps = None def delta_trunctate( (x,y,z, axis, angle) ): # angle not more than 35 deg if angle < math.radians(-45): angle = math.radians(-45) if angle > math.radians(45): angle = math.radians(45) # if angle < 5 deg than trunc, else shift if abs(angle) < math.radians(5): angle = 0.0 axis = [0.0, 0.0, 0.0] else: # 6->1 # 7->2 angle = angle - (1.0 if angle > 0 else -1.0)*math.radians(5) # only one angle allowed if False: axisabs = map(abs,axis) maxidx = axisabs.index( max(axisabs) ) angle = axis[maxidx]*angle axis = [0.0, 0.0, 0.0] axis[maxidx] = 1.0 # if angle is z, than no move #if axis[2]: # x,y,z = 0,0,0 return (x,y,z,axis,angle) def move_update(cs, points): global old_ps, mid_ps, delta_position ## ignore if not old_ps and not points: return ## begin if old_ps is None and points: old_ps = count_points(cs, points) old_ps = sort_ps_by_z(old_ps) mid_ps = old_ps return ## end if old_ps and not points: old_ps = None mid_ps = None update_robot_position(delta_position, commit=True) return ## got data new_ps = count_points(cs, points) new_psa = new_ps new_ps = sort_ps_by_pair(new_ps, mid_ps) mid_ps = new_ps pa0 = old_ps[0] pb0 = new_ps[0] translation = (pb0[0] - pa0[0], pb0[1] - pa0[1], pb0[2] - pa0[2]) if len(old_ps) > 1 and len(new_ps) > 1: pa1 = old_ps[1] pb1 = new_ps[1] vec_before = (pa1[0] - pa0[0], pa1[1] - pa0[1], pa1[2] - pa0[2]) vec_after = (pb1[0] - pb0[0], pb1[1] - pb0[1], pb1[2] - pb0[2]) N = crossproduct(vec_before, vec_after) N, _ = unitize(N) axis = N angle = angle_between_vectors(vec_before, vec_after, N) else: axis = [0,0,0] angle = 0 #<-0.5, 0,5> ---> <-50MM, 50MM># xd, yd, zd = map(lambda a:(a**2)*1200.0*(1.0 if a > 0 else -1.0), [translation[0], -translation[1], -translation[2]]) delta_position = [xd, yd, zd, axis, angle] delta_position = delta_trunctate(delta_position) update_robot_position(delta_position) return pb0 mem = None def setmem(nmem): global mem mem = nmem update_robot(robot_position) def update_robot(pos): global mem x,y,z,axis, angle = pos a,b,g = axis_angle_to_deg(axis,angle) x,y,z = map(lambda p:int(p*1000.0),[x,y,z]) a,b,g = map(lambda p:int(p*100.0), [a,b,g]) if mem: mem.write(struct.pack("iiiiii",x,y,z,a,-b,-g)) else: print "no shared mem!!!"