# Script to calculate a homography
# Fraga
# 08.05.2025
#
import numpy as np
from numpy.linalg import qr

# Read the input points
A = np.loadtxt("p1.txt")
n, m = A.shape
va = np.ones( (n,1) )
P1 = np.hstack( (A, va) )
# print( P1.shape )
# print( P1 )
B = np.loadtxt("p2.txt")
n, m = B.shape
vb = np.ones( (n,1) )
P2 = np.hstack( (B, vb) )

# Calculate mean and standard deviation
# of input points
m1 = np.mean( P1, axis=0 )
s1 = np.std( P1, axis=0 )

m2 = np.mean( P2, axis=0 )
s2 = np.std( P2, axis=0 )

# H = T1^{-1} H' T2

T1inv = np.array( [ [s1[0],   0.0, m1[0] ],
                    [  0.0, s1[1], m1[1] ],
                    [  0.0,   0.0,   1.0 ] ] ) 

T1 = np.array( [ [1/s1[0],   0.0  , -m1[0]/s1[0] ],
                 [  0.0,   1/s1[1], -m1[1]/s1[1] ],
                 [  0.0,     0.0,        1.0 ] ] ) 

T2 = np.array( [ [1/s2[0],   0.0  , -m2[0]/s2[0] ],
                 [  0.0,   1/s2[1], -m2[1]/s2[1] ],
                 [  0.0,     0.0,        1.0 ] ] ) 

P1p = np.transpose( T1 @ P1.T )
#                   3x3  3x4 
#                     3x4 
#          4x3
P2p = np.transpose( T2 @ P2.T )

# A matriz and vb vector

A = np.zeros( (8,8) )
vb = np.zeros( (8,1) )

k=0
while k<4 :
	x1 = P1p[k,0]
	y1 = P1p[k,1]

	x2 = P2p[k,0]
	y2 = P2p[k,1]

	i = 2*k
	A[i,0]=x2;  A[i,1]=y2; A[i,2]=1.0; A[i,3]=0.0; A[i,4]=0.0; A[i,5]=0.0; A[i,6] = -x1*x2; A[i,7]=-x1*y2
	vb[i,0] = x1
	i = 2*k + 1
	A[i,0]=0.0; A[i,1]=0.0; A[i,2]=0.0; A[i,3]=x2; A[i,4]=y2; A[i,5]=1.0; A[i,6] = -y1*x2; A[i,7]=-y1*y2
	vb[i,0] = y1

	k += 1

B = np.hstack( (A, vb) )
R = qr( B, mode='r' ) 

# It the last column of B is the multiplication of Q^T b
n, m = A.shape

# To obtain the result, we apply the backsustitution
vh = np.zeros( (n,) )

n1 = n-1
vh[n1] = R[n1,n] / R[n1,n1]

i = n1-1
while i >= 0 :
	mysum = 0.0
	j = i+1
	while j<n :
		mysum += R[i,j]*vh[j]
		j += 1

	vh[i] = (R[i,n] - mysum)/R[i,i]
	i -= 1

Hp = np.array( [[ vh[0], vh[1], vh[2] ],
                [ vh[3], vh[4], vh[5] ],
                [ vh[6], vh[7],  1.0 ] ] )

H = T1inv @ Hp @ T2
print( H )