LJ/libs3/homographyp.py

253 lines
7.5 KiB
Python
Raw Permalink Normal View History

2020-09-19 12:28:56 +00:00
#!/usr/bin/python3
2018-12-13 11:05:32 +00:00
# -*- coding: utf-8 -*-
# -*- mode: Python -*-
'''
LJay/LJ
v0.7.0
LICENCE : CC
Sam Neurohack
Homographies for align + swap corrections and 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
2020-09-19 12:28:56 +00:00
from libs3 import gstt
2018-12-13 11:05:32 +00:00
#from globalVars import xy_center
import redis
r = redis.StrictRedis(host=gstt.LjayServerIP, 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')
2020-09-19 12:28:56 +00:00
for i in range(npoints):
2018-12-13 11:05:32 +00:00
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')
2020-09-19 12:28:56 +00:00
for i in range(npoints):
2018-12-13 11:05:32 +00:00
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([(300.0, 400.0), (500.0, 400.0), (500.0, 200.0), (300.0, 200.0)])
2020-09-19 12:28:56 +00:00
def EDpoint(mylaser, xxx_todo_changeme):
2018-12-13 11:05:32 +00:00
#print "current point : ", pygamex, pygamey
2020-09-19 12:28:56 +00:00
(pygamex,pygamey) = xxx_todo_changeme
2018-12-13 11:05:32 +00:00
XX = pygamex - gstt.xy_center[0]
YY = pygamey - gstt.xy_center[1]
CosANGLE = math.cos(gstt.finANGLE[mylaser])
SinANGLE = math.sin(gstt.finANGLE[mylaser])
2019-01-16 00:50:24 +00:00
2018-12-13 11:05:32 +00:00
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]
2018-12-18 01:45:23 +00:00
if gstt.debug >1:
2018-12-13 11:05:32 +00:00
#print "global center :", xy_center
2020-09-19 12:28:56 +00:00
print("EDpoint computing...")
print("Laser :", mylaser, "center at : ", gstt.centerX[mylaser], gstt.centerY[mylaser])
print("Pygame point",pygamex,",",pygamey)
2018-12-13 11:05:32 +00:00
'''
print "swaps : ", (gstt.swapX[mylaser]), str(gstt.swapY[mylaser])
print "zooms : ", gstt.zoomX[mylaser], gstt.zoomY[mylaser]
print "angles : ", gstt.finANGLE[mylaser]
'''
2020-09-19 12:28:56 +00:00
print("Result point : ", x * gstt.swapX[mylaser] , y * gstt.swapY[mylaser])
2018-12-13 11:05:32 +00:00
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 : ED (= align + swap) transform + warp transform.
# WARP IS DISABLED. Some bug tracking is needed !
def newEDH(mylaser):
EDpoints = []
2020-09-19 12:28:56 +00:00
for point in range(4):
2018-12-13 11:05:32 +00:00
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)
2020-09-19 12:28:56 +00:00
print("Tracer", mylaser, ": new EDH computed, sending to redis...")
2018-12-15 19:03:32 +00:00
if r.set('/EDH/'+str(mylaser), np.array2string(gstt.EDH[mylaser], separator=',')) == True:
r.set('/order/'+str(mylaser), 1)
2020-09-19 12:28:56 +00:00
print("New EDH sent.")
2018-12-15 19:03:32 +00:00
else:
2020-09-19 12:28:56 +00:00
print("New EDH not sent.")
2018-12-15 19:03:32 +00:00
'''
2018-12-13 11:05:32 +00:00
# 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))
2018-12-15 19:03:32 +00:00
'''
2018-12-13 11:05:32 +00:00
if gstt.debug >1:
2020-09-19 12:28:56 +00:00
print("")
print("laser ", mylaser)
print("reference points", pointsref)
print("laser EDpoints :", EDpoints)
print("-> Computed H :",H)
2018-12-13 11:05:32 +00:00
#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 ""
2020-09-19 12:28:56 +00:00
print("-> new EDH :", gstt.EDH[mylaser])
2018-12-13 11:05:32 +00:00