import numpy as np
import open3d as o3d
from utils.get_pose_matrix import get_c2w,get_w2c

def get_cam_infos(images: dict, cameras:dict):
    cam_infos = {}
    for image_id, image_info in images.items():
        camera_info = cameras[image_info.camera_id]
        fx = camera_info.params[0]
        fy = camera_info.params[1]
        cx = camera_info.params[2]
        cy = camera_info.params[3]
        width = camera_info.width
        height = camera_info.height
        name = image_info.name
        cam_infos[image_id] = {
            "name": name,
            "w2c": get_w2c(image_info.qvec, image_info.tvec),
            "c2w": get_c2w(image_info.qvec, image_info.tvec),
            "width": camera_info.width,
            "height": camera_info.height,
            "intrinsics": o3d.camera.PinholeCameraIntrinsic(
                width, 
                height,
                fx=fx,
                fy=fy,
                cx=cx,
                cy=cy
            ),
            "K": np.array([
                [fx, 0, cx],
                [0, fy, cy],
                [0, 0, 1]
            ])
        }
    return cam_infos