protonphoton
/
LJ
6
0
Fork 1
Code Issues Pull Requests 1 Releases Wiki Activity
LJ/libs3/homographyp.py

273 lines
8.3 KiB
Python
Raw Permalink Blame History

#!/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])
Reference in New Issue View Git Blame Copy Permalink