import cv2
import numpy as np
import scipy.io as scio

if __name__ == '__main__':
    print("main function.")
    #验证点
    point = np.array([1.0 ,2.0, 3.0])
    #获取相机参数
    cams_data = scio.loadmat('/data1/dy/SuperSMPL/data/AMAfMvS_Dataset/cameras_I_crane.mat')
    Pmats = cams_data['Pmats']  # Pmats(8, 3, 4) 投影矩阵 
    P1 = Pmats[0,::]
    P3 = Pmats[2,::]
    #通过投影矩阵将点从世界坐标投到像素坐标
    pj1 = np.dot(P1, np.vstack([point.reshape(3,1),np.array([1])]))
    pj3 = np.dot(P3, np.vstack([point.reshape(3,1),np.array([1])]))
    point1 = pj1[:2,:]/pj1[2,:]#两行一列，齐次坐标转化
    point3 = pj3[:2,:]/pj3[2,:]
    #利用投影矩阵以及对应像素点，进行三角测量
    points = cv2.triangulatePoints(P1,P3,point1,point3)
    #齐次坐标转化并输出
    print(points[0:3,:]/points[3,:])