# coding: utf-8
import pylab as pl

def normalize(points):
 """Normiliza un conjunto de puntos en coordenadas homogeneas,
 haciendo 1 la ultima fila"""

 for row in points:
   row /= points[-1]
 return points

def make_homog(points):
  """Convierte un conjunto de puntos (de un array de dim*n) a 
  coordenadas homogeneas"""

  return pl.vstack( (points, pl.ones( (1, points.shape[1]))))

def H_from_points(fp,tp):
  """Calcula la homografía que lleva fp a tp usando DLT.
  Los puntos son condicionados tal que tengan media cero y 
  desviación estandar 1"""

  if fp.shape != tp.shape:
    raise RunTimeError('El número de puntos no coincide')

  # normalización de puntos
  # -- from points --
  m = pl.mean(fp[:2], axis=1)
  maxstd = max(pl.std(fp[:2], axis=1)) + 1e-9
  C1 = pl.diag([1/maxstd, 1/maxstd, 1])
  C1[0][2] = -m[0]/maxstd
  C1[1][2] = -m[1]/maxstd
  fp = pl.dot(C1,fp)

  # -- to points --
  m = pl.mean(tp[:2], axis=1)
  maxstd = max(pl.std(tp[:2], axis=1)) + 1e-9
  C2 = pl.diag([1/maxstd, 1/maxstd, 1])
  C2[0][2] = -m[0]/maxstd
  C2[1][2] = -m[1]/maxstd
  tp = pl.dot(C2,tp)


  # crear la matriz para DLT, dos filas para cada par de correspondencias
  nbr_correspondences = fp.shape[1]
  A = pl.zeros((2*nbr_correspondences, 9))
  for i in range(nbr_correspondences):
    A[2*i] = [-fp[0][i],-fp[1][i],-1,0,0,0,
	tp[0][i]*fp[0][i],tp[0][i]*fp[1][i],tp[0][i]]
    A[2*i+1] = [0,0,0,-fp[0][i],-fp[1][i],-1,
	tp[1][i]*fp[0][i],tp[1][i]*fp[1][i],tp[1][i]]
  
  U,S,V = pl.linalg.svd(A)
  H = V[8].reshape((3,3))

  # denormalización
  H = pl.dot(pl.linalg.inv(C2), pl.dot(H,C1))
  return H / H[2,2]


def Haffine_from_points_DLT(fp,tp):
  """ Calcula la homografía que lleva fp a tp usando DLT.
  Los puntos son condicionados tal que tengan media cero y 
  desviación estandar 1 """

  if fp.shape != tp.shape:
    raise RunTimeError('El número de puntos no coincide')

  # normalización de puntos
  # -- from points --
  m = pl.mean(fp[:2], axis=1)
  maxstd = max(pl.std(fp[:2], axis=1)) + 1e-9
  C1 = pl.diag([1/maxstd, 1/maxstd, 1])
  C1[0][2] = -m[0]/maxstd
  C1[1][2] = -m[1]/maxstd
  fp = pl.dot(C1,fp)

  # -- to points --
  m = pl.mean(tp[:2], axis=1)
  maxstd = max(pl.std(tp[:2], axis=1)) + 1e-9
  C2 = pl.diag([1/maxstd, 1/maxstd, 1])
  C2[0][2] = -m[0]/maxstd
  C2[1][2] = -m[1]/maxstd
  tp = pl.dot(C2,tp)


  # crear la matriz para DLT, dos filas para cada par de correspondencias
  nbr_correspondences = fp.shape[1]
  A = pl.zeros((2*nbr_correspondences, 7))
  for i in range(nbr_correspondences):
    A[2*i] = [-fp[0][i],-fp[1][i],-1,0,0,0,tp[0][i]]
    A[2*i+1] = [0,0,0,-fp[0][i],-fp[1][i],-1,tp[1][i]]
  
  U,S,V = pl.linalg.svd(A)
  # para transfromación afín h7 y h8 = 0
  B=V[6]
  B= pl.concatenate((B[:6],[0,0],[B[6]]))
  H = B.reshape((3,3))

  # denormalización
  H = pl.dot(pl.linalg.inv(C2), pl.dot(H,C1))
  return H / H[2,2]


