'''
Description: 
继matlab标定后，使用OpenCV进行手眼标定；
首先使用matlab camera calibrate 工具箱进行标定，保存标定文件cameraParams.mat；
'''
from scipy.io import loadmat
import cv2
import numpy as np
from math import *
from utils_py.mat2ini import load_mat_to_cameraParams
import utils_py.utils_trans as utils_trans

def eye_in_hand_calibrate(dir_path = "./eye_in_hand_cali_data"):

    # 设置 NumPy 的打印选项，禁用科学计数法
    np.set_printoptions(suppress=True)

    # 读取matlab标定文件
    mat_file_path = f'{dir_path}/cameraParams.mat'

    calibrate_dict = load_mat_to_cameraParams(mat_file_path, f'{dir_path}/camera.ini')

    RotationVectors = calibrate_dict['RotationVectors'][0]

    TranslationVectors = calibrate_dict['TranslationVectors'][0]

    #计算board to cam 变换矩阵
    R_all_chess_to_cam_1=[]
    T_all_chess_to_cam_1=[]
    for i in range(len(RotationVectors)):
        R_chess_to_cam, _ = cv2.Rodrigues(np.array(RotationVectors[i]))
        R_all_chess_to_cam_1.append(R_chess_to_cam)
        T_all_chess_to_cam_1.append(np.array(TranslationVectors[i]))

    #计算end to base变换矩阵
    R_all_end_to_base_1=[]
    T_all_end_to_base_1=[]    
    end_pose = utils_trans.read_pose_from_txt(f'{dir_path}/end_pose.txt')

    for pose in end_pose:
        RT = utils_trans.xyzRxRyRz_to_RT(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5])
        R_all_end_to_base_1.append(RT[:3, :3])
        T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))

    R,T=cv2.calibrateHandEye(R_all_end_to_base_1, T_all_end_to_base_1, R_all_chess_to_cam_1, T_all_chess_to_cam_1)#手眼标定

    print("手眼矩阵分解得到的旋转矩阵")
    print(R)
    print("\n")


    print("手眼矩阵分解得到的平移矩阵")
    print(T)

    RT=np.column_stack((R,T))
    RT = np.row_stack((RT, np.array([0, 0, 0, 1])))#即为cam to end变换矩阵
    print("\n")
    print('相机相对于末端的变换矩阵为：')
    print(RT)
    # 指定保存的文件路径
    file_path = f"{dir_path}/cam2end.txt"

    # 将矩阵写入文本文件
    with open(file_path, 'w') as file:
        for row in RT:
            row_str = ' '.join(map(str, row))  # 将矩阵的每一行转换为字符串
            file.write(row_str + '\n')  # 写入文件并添加换行符


    #结果验证，原则上来说，每次结果相差较小
    for i in range(len(RotationVectors)):

        # 得到机械手末端到基座的变换矩阵，通过机械手末端到基座的旋转矩阵与平移向量先按列合并，然后按行合并形成变换矩阵格式
        RT_end_to_base=np.column_stack((R_all_end_to_base_1[i],T_all_end_to_base_1[i]))
        RT_end_to_base=np.row_stack((RT_end_to_base,np.array([0,0,0,1])))
        # print(RT_end_to_base)

        # 标定版相对于相机的齐次矩阵
        RT_chess_to_cam=np.column_stack((R_all_chess_to_cam_1[i],T_all_chess_to_cam_1[i]))
        RT_chess_to_cam=np.row_stack((RT_chess_to_cam,np.array([0,0,0,1])))
        # print(RT_chess_to_cam)

        # 手眼标定变换矩阵
        RT_cam_to_end=np.column_stack((R,T))
        RT_cam_to_end=np.row_stack((RT_cam_to_end,np.array([0,0,0,1])))
        # print(RT_cam_to_end)

        # 即为固定的棋盘格相对于机器人基坐标系位姿
        RT_chess_to_base=RT_end_to_base@RT_cam_to_end@RT_chess_to_cam
        RT_chess_to_base=np.linalg.inv(RT_chess_to_base)
        print('第',i,'次')

        print(f"{RT_chess_to_base[:3,:]}")
        print('')

if __name__ == '__main__':
    dir_path = "./eye_in_hand_cali_data"
    eye_in_hand_calibrate(dir_path)