#!/usr/bin/python3 # -*- coding: utf-8 -*- # -*- mode: Python -*- ''' LJay/LJ v0.7.0 LICENCE : CC Sam Neurohack Homographies for LJ settings corrections and DISABLED warp corrections Align + swap homography if found with 4 original points and corrected coordinates Warp correction is disabled for the moment. Should be computed at warp edition : set 1 curve 1 Use the : ######################################################################## # Module to compute homographies # # # # Author : Alexis Mignon # # email : alexis.mignon@info.unicaen.fr # # date : 10/03/2010 # ######################################################################## Module to compute homographies between two sets of 2D points implemented functions : - find_homography(points1,points2) : finds the homography between two sets of 2D points - find_affine_homography(points1,points2) : finds the affine homography between two sets of 2D points - apply_homography(H,points) : applies homography H to the set of 2D points 'points' example : >>> from homography import * >>> >>> points1 = np.array([[ 0., 0. ], >>> [ 1., 0. ], >>> [ 0., 1. ], >>> [ 1., 1. ]]) >>> >>> points2 = np.array([[ 0. , 0. ], >>> [ 1. , 0. ], >>> [ 0.25, 1. ], >>> [ 0.75, 1. ]]) >>> >>> points3 = np.array([[-1., 0.], >>> [ 0.,-1.], >>> [ 0., 1.], >>> [ 1., 0.]]) >>> >>> H1 = find_homography(points1,points2) >>> print H1 >>> print apply_homography(H1,points1) >>> H2 = find_affine_homography(points1,points3) >>> print H2 >>> print apply_homography(H2,points1) ''' import numpy as np import math from scipy.linalg import svd,lstsq import ast from libs3 import gstt from libs3 import commands #from globalVars import xy_center import redis r = redis.StrictRedis(host=gstt.RediServerIP, port=6379, db=0) def find(points1,points2): if points1.shape[0] != points2.shape[0] : raise ValueError("The number of input and output points mismatches") if points1.shape[1] == 2 : p1 = np.ones((len(points1),3),'float64') p1[:,:2] = points1 elif points1.shape[1] == 3 : p1 = points1 else : raise ValueError("Bad shape for input points") if points2.shape[1] == 2 : p2 = np.ones((len(points2),3),'float64') p2[:,:2] = points2 elif points2.shape[1] == 3 : p2 = points2 else : raise ValueError("Bad shape for output points") npoints = len(points1) A = np.zeros((3*npoints,9),'float64') for i in range(npoints): p1i = p1[i] x2i,y2i,w2i = p2[i] xpi = x2i*p1i ypi = y2i*p1i wpi = w2i*p1i A[i*3 ,3:6] = -wpi A[i*3 ,6:9] = ypi A[i*3+1,0:3] = wpi A[i*3+1,6:9] = -xpi A[i*3+2,0:3] = -ypi A[i*3+2,3:6] = xpi U,s,Vt = svd(A,full_matrices = False, overwrite_a = True) del U,s h = Vt[-1] H = h.reshape(3,3) return H def find_affine(points1,points2): if points1.shape[0] != points2.shape[0] : raise ValueError("The number of input and output points mismatches") if points1.shape[1] == 2 : p1 = np.ones((len(points1),3),'float64') p1[:,:2] = points1 elif points1.shape[1] == 3 : p1 = points1 else : raise ValueError("Bad shape for input points") if points2.shape[1] == 2 : p2 = np.ones((len(points2),3),'float64') p2[:,:2] = points2 elif points2.shape[1] == 3 : p2 = points2 else : raise ValueError("Bad shape for output points") npoints = len(points1) A = np.zeros((3*npoints,6),'float64') b = np.zeros((3*npoints,1),'float64') for i in range(npoints): p1i = p1[i] x2i,y2i,w2i = p2[i] xpi = x2i*p1i ypi = y2i*p1i wpi = w2i*p1i A[i*3 ,3:6] = -wpi A[i*3+1,0:3] = wpi A[i*3+2,0:3] = -ypi A[i*3+2,3:6] = xpi b[i*3 ] = -y2i*p1i[2] b[i*3+1] = x2i*p1i[2] h = lstsq(A,b,overwrite_a = True, overwrite_b = True)[0] H = np.zeros( (3,3) , 'float64' ) H[:2,:] = h.reshape(2,3) H[2,2] = 1 return H def apply(H,points): p = np.ones((len(points),3),'float64') p[:,:2] = points pp = np.dot(p,H.T) pp[:,:2]/=pp[:,2].reshape(len(p),1) return pp[:,:2] # Align and axis swap corrections # Reference points pointsref = np.array([(-1500.0, 1500.0), (1500.0, 1500.0), (1500.0, -1500.0), (-1500.0, -1500.0)]) # wad : pointsref = np.array([(300.0, 400.0), (500.0, 400.0), (500.0, 200.0), (300.0, 200.0)]) ScreenX = [-1500,1500] ScreenY = [-1500,1500] # Compute refpoint transformation in EtherDream space according to settings webpage (= LJ.conf) def EDpoint(mylaser, refpt): XX = refpt[0] YY = refpt[1] CosANGLE = math.cos(gstt.finANGLE[mylaser]) SinANGLE = math.sin(gstt.finANGLE[mylaser]) #if zoom is a percentage of 65535 etherdream space #x = (((XX * CosANGLE) - (YY * SinANGLE))) * gstt.zoomX[mylaser] * 65535 + gstt.centerX[mylaser] #y = (((XX * SinANGLE) + (YY * CosANGLE))) * gstt.zoomY[mylaser] * 65535 + gstt.centerY[mylaser] x = (((XX * CosANGLE) - (YY * SinANGLE))) * gstt.zoomX[mylaser] + gstt.centerX[mylaser] y = (((XX * SinANGLE) + (YY * CosANGLE))) * gstt.zoomY[mylaser] + gstt.centerY[mylaser] ''' Was : (pygamex,pygamey) = refpt XX = pygamex - gstt.xy_center[0] YY = pygamey - gstt.xy_center[1] CosANGLE = math.cos(gstt.finANGLE[mylaser]) SinANGLE = math.sin(gstt.finANGLE[mylaser]) x = (gstt.xy_center[0] + ((XX * CosANGLE) - (YY * SinANGLE)) - gstt.xy_center[0]) * gstt.zoomX[mylaser] + gstt.centerX[mylaser] y = (gstt.xy_center[1] + ((XX * SinANGLE) + (YY * CosANGLE)) - gstt.xy_center[1]) * gstt.zoomY[mylaser] + gstt.centerY[mylaser] ''' #print() #print("EDpoint computing...") #print("Reference point :",refpt[0], refpt[1]) #print("Projected point :", x * gstt.swapX[mylaser] , y * gstt.swapY[mylaser]) #print("Laser :", mylaser) #print("Reference center : 0 0") #print("swaps :", gstt.swapX[mylaser], gstt.swapY[mylaser]) #print("zooms :", gstt.zoomX[mylaser], gstt.zoomY[mylaser]) #print("angle :", gstt.finANGLE[mylaser]) #print("Projection center at :", gstt.centerX[mylaser], gstt.centerY[mylaser]) commands.UpdateOSCUI(mylaser) return [x * gstt.swapX[mylaser] , y * gstt.swapY[mylaser]] ''' def EDpoint((pygamex,pygamey)): XX = pygamex - xy_center[0] YY = pygamey - xy_center[1] CosANGLE = math.cos(finangle) SinANGLE = math.sin(finangle) # Multilaser style x = (xy_center[0] + ((XX * CosANGLE) - (YY * SinANGLE)) - xy_center[0]) * zoomx + centerx y = (xy_center[1] + ((XX * SinANGLE) + (YY * CosANGLE)) - xy_center[1]) * zoomy + centery return [x*1, y*1] ''' # New total homography from always the same reference points to EtherDream space according to LJ settings transform + warp DISABLED transform. # WARP IS DISABLED. Some bug tracking is needed ! def newEDH(mylaser): EDpoints = [] for point in range(4): EDpoints.append(EDpoint(mylaser,pointsref[point])) # H matrix tansform pygame points in Etherdream system with align and swap correction, H = find(pointsref, np.array(EDpoints)) # Computer Hwarp matrix with previously reference warped points in configuration file. Hwarp = find(pointsref, gstt.warpdest[mylaser]) #Hwarp = np.identity(3, dtype = float) # EDH matrix gstt.EDH[mylaser] = H # EDH matrix is H x Hwarp #gstt.EDH[mylaser] = np.dot(H,Hwarp) print("Tracer", mylaser, ": new EDH computed, sending to redis...") if r.set('/EDH/'+str(mylaser), np.array2string(gstt.EDH[mylaser], separator=',')) == True: r.set('/order/'+str(mylaser), 1) print("New EDH sent.") print() else: print("New EDH not sent.") print() ''' # Laser bit 0 = 0 and bit 1 = 1 : New EDH order = r.get('/order') print order neworder = order & ~(1<< mylaser*2) neworder = neworder | (1<< 1+mylaser*2) r.set('/order', str(neworder)) ''' if gstt.debug >1: print("") print("laser ", mylaser) print("reference points", pointsref) print("laser EDpoints :", EDpoints) print("-> Computed H :",H) #print "warped points coordinates ", gstt.warpdest[mylaser] #print "-> Computed Hwarp", Hwarp #print "laser ", mylaser, "warpd ",ast.literal_eval(gstt.warpdest[gstt.Laser]) #print "laser ", mylaser, "Hwarp ", Hwarp #print "" print("-> new EDH :", gstt.EDH[mylaser])