import numpy as np
import mat_rot as mr
def rt_ref(Rvec1,Tvec1,Rvec2,Tvec2):
     Rmat1=mr.MatRot(Rvec1)
     Rmat2=mr.MatRot(Rvec2)
     H_10=np.identity(4)
     H_20=np.identity(4)
     H_12=np.identity(4)
     #H_10[:3,:3]=Rmat1.T
     #H_20[:3,:3]=Rmat2.T
     #H_10[3,:3]=Tvec1[:,0]
     #H_20[3,:3]=Tvec2[:,0]
     H_10[:3,:3]=Rmat1
     H_20[:3,:3]=Rmat2
     H_10[:3,3]=Tvec1[:,0]
     H_20[:3,3]=Tvec2[:,0]
     #print H_10
     H_20_inv=np.linalg.inv(H_20)
     H_12=np.dot(H_10,H_20_inv)
     #Rmat2_inv=np.linalg.inv(Rmat2)
     #Rmat1=Rmat1.T
     #Rmat1_inv=Rmat2_inv.T
     #Rmat12=np.dot(Rmat2_inv,Rmat1)
     #Tvec12=Tvec1-np.dot(np.dot(Rmat2_inv,Rmat1),Tvec2)
     #Rmat12=np.dot(Rmat2_inv,Rmat1)
     #Tvec12=Tvec1-np.dot(Tvec2,np.dot(Rmat2_inv,Rmat1))
     #Rmat12=H_12[:3,:3]
     #Tvec12=H_12[3,:3]
     Rmat12=H_12[:3,:3]
     Tvec12=H_12[:3,3]
     return Rmat12,Tvec12

# Ejemplo de uso de la funcion:
#Rvec1=np.array([[10],[0],[0]])
#Rvec2=np.array([[10],[0],[0]])
#Tvec1=np.array([[15],[7],[35]])
#Tvec2=np.array([[15],[7],[35]])

#Rmat12,Tvec12=rt_ref(Rvec1,Tvec1,Rvec2,Tvec2)
#print Rmat12
#print Tvec12
