import cv2
import numpy as np
import imageio
import open3d as o3d
from tifffile import tifffile
# 相机内参矩阵 [fx, 0, cx; 0, fy, cy; 0, 0, 1]
K = np.array([
    [1.30449e3, 0,         5.2602e2],
    [0,         1.30449e3, 1.07432e3],
    [0,         0,         1]
])
fx, fy = K[0, 0], K[1, 1]
cx, cy = K[0, 2], K[1, 2]

def pointscloud2depthmap(points):
    # 初始化一个空的目标数组
    point_image = np.zeros((height, width, 3), dtype=np.float32)
    # 遍历点云中的每个点，进行投影并填充目标数组
    for point in points:
        X, Y, Z ,r,g,b= point
        if Z > 0:  # 确保Z值有效
            # 计算2D图像坐标
            u = int((X * fx) / Z + cx)
            v = int((Y * fy) / Z + cy)

            # 检查是否在图像边界内
            if 0 <= u < width and 0 <= v < height:
                point_image[v, u, :] = (X,Y,Z)

    return point_image
def pointscloud2colorimg(points):
    # 初始化一个空的目标数组
    point_image = np.zeros((height, width, 3), dtype=np.float32)
    color_image = np.zeros((height, width, 3), dtype=np.float32)
    # 遍历点云中的每个点，进行投影并填充目标数组
    for point in points:
        X, Y, Z ,r,g,b= point
        if Z > 0:  # 确保Z值有效
            # 计算2D图像坐标
            u = int((X * fx) / Z + cx)
            v = int((Y * fy) / Z + cy)

            # 检查是否在图像边界内
            if 0 <= u < width and 0 <= v < height:
                # point_image[v, u, :] = point
                color_image[v,u]=[r*255,g*255,b*255]

    return color_image


# # 使用imageio读取
# loaded_depth_map = imageio.v3.imread(r"depth_map.tiff")
# print(loaded_depth_map.shape)
# print(loaded_depth_map.dtype)
# # print(loaded_depth_map)




# 加载PCD文件
pcd = o3d.io.read_point_cloud(r"F:\test_pointcloud\color2.pcd")

# 打印点的数量
print("Number of points:", len(pcd.points))

# 获取点云数据
points = np.asarray(pcd.points)

colors=np.asarray(pcd.colors)
points = np.hstack((points, colors))  # (N, 6)

# 打印前5个点的坐标
print("First 5 points:\n", points[:5])
print(f'color:{colors}')
#
# print(f'depth :{loaded_depth_map[0,0:5]}')
#
#
# print(loaded_depth_map[102,113])
#
# # 将深度图转换为点云
# height ,width = loaded_depth_map.shape[:2]
# print(f'height:{height},width:{width}')


# point_cloud_from_depth = []
# for v in range(height):
#     for u in range(width):
#         x_,y_,z_=loaded_depth_map[v,u]
#         print(f'x_,y_,z_:({x_},{y_},{z_})')
#         depth = loaded_depth_map[v, u][-1]
#         print(f'depth:{depth}')
#         # if depth > 0:  # 忽略无效的深度值
#         x = (u - cx) * depth / fx
#         y = (v - cy) * depth / fy
#         z = depth
#         print(f'x,y,z:({x},{y},{z})')
#         point_cloud_from_depth.append([x, y, z])
#
# point_cloud_from_depth = np.array(point_cloud_from_depth)
#
# # 打印从深度图生成的点云中的前5个点
# print("First 5 points from depth map:\n", point_cloud_from_depth[:5])


# 目标深度图尺寸
height, width = 2000, 2000







# point_image=pointscloud2depthmap(points)
point_image=pointscloud2colorimg(points)
# 打印结果以验证
print("Shape of the projected point cloud:", point_image.shape)
print("First few pixels (if any):", point_image[:5, :5, :])

# 提取 Z 值作为深度图
depth_map = point_image[:, :, 2]
# depth_map=point_image
# 处理无效点（例如，设置无效点的深度值为一个极大值）
# invalid_depth_value = np.max(depth_map) * 2  # 或者选择其他合适的值
# depth_map[depth_map == 0] = invalid_depth_value  # 将所有无效点（Z=0）替换为极大值

# 打印深度图的一些信息以验证
print("Depth map shape:", depth_map.shape)
print("Depth map dtype:", depth_map.dtype)
print("Min depth value:", np.min(depth_map))
print("Max depth value:", np.max(depth_map))

# 保存为 TIFF 文件
# output_tiff_path = 'depth_map.tiff'
color_img_path='color_img.jpg'

# tifffile.imwrite(output_tiff_path, depth_map.astype(np.float16))
cv2.imwrite(color_img_path,point_image)
# print(f"Depth map saved to {output_tiff_path}")