# Estimación de transformación afín de 1 a 2 empleando solo "inliers"
def estimar_afinidad(p1, p2):
    A_list = []
    b_list = []
    for p in p1:
        a1 = [p[0], p[1], p[2], 0., 0., 0.]
        a2 = [0., 0., 0., p[0], p[1], p[2]]
        A_list.append(a1)
        A_list.append(a2)
    A = np.array(A_list)
    for p in p2:
        b_list.append([p[0]])
        b_list.append([p[1]])
    b = np.array(b_list)

    t1 = A.T
    print(t1.shape)
    t2 = np.matmul(t1, A)
    print(t2.shape)
    t3 = np.linalg.inv(t2)

    #t4 = np.matmul(t3, t1)
    #tr = t4.dot(b)

    #tr = np.linalg.inv(np.matmul(A.T, A)).dot(A.T.dot(b))

    tr = np.eye(3)
    # ToDO (usar solución en forma cerrada)
    return tr

p1_inliers = p1[inliers, :]
p2_inliers = p2[inliers, :]

# estimación de transformación en coordenadas normalizadas
A = estimar_afinidad(p1_inliers, p2_inliers)

imshow2(img1, img2, A)