通过这个代码生成mask，获取相机坐标，图片信息，投影的过程，写个代码，让照片中的class为wall的pixel和相机中心形成一条射线，以相机中心为顶点延伸至无限长，然后这条射线会与0.793553x + -0.040436y + 0.607156z + -2.950997 = 0有个交点，把这个交点生成新的点云的点，然后和之前的ply模型中的点云的点融合形成新的点云模型，记住，相机不要000，要通过参数计算出相机中心的坐标！！！！选取的pixel必须是wall范围里的，选取的点要足够多，最好能覆盖wall区！！！必要时可以在open3d输出时标出射线情况和新的点的位置，最后再原有的点云模型上标注出新点，总而言之，从XML文件中读取相机的外参（即相机的变换矩阵）和标定数据，并使用这些信息生成投影，形成射线并与给定的平面相交，然后将交点作为新的点云点与原始点云融合！！！每一步加上步骤和运行进度，import os
import numpy as np
import xml.etree.ElementTree as ET
from pycocotools.coco import COCO
import cv2
import open3d as o3d
from numpy.linalg import inv
import math

# Function to read camera transformations from XML
def read_camera_transforms(xml_file_path):
    tree = ET.parse(xml_file_path)
    root = tree.getroot()
    camera_info = []
    for camera in root.iter('camera'):
        camera_id = camera.attrib.get('id')
        sensor_id = camera.attrib.get('sensor_id', 'default')
        label = camera.attrib.get('label', 'No Label')
        transform_text = camera.find('transform').text if camera.find('transform') is not None else "N/A"
        camera_info.append({
            'camera_id': camera_id,
            'label': label,
            'sensor_id': sensor_id,
            'transform': transform_text
        })
    return camera_info

# Function to read camera calibrations from XML
def read_camera_calibrations(xml_file_path):
    tree = ET.parse(xml_file_path)
    root = tree.getroot()
    sensor_calibrations = {}
    for calibration in root.iter('calibration'):
        try:
            f = float(calibration.find('f').text) if calibration.find('f') is not None and calibration.find('f').text else 0
            cx = float(calibration.find('cx').text) if calibration.find('cx') is not None and calibration.find('cx').text else 0
            cy = float(calibration.find('cy').text) if calibration.find('cy') is not None and calibration.find('cy').text else 0
            k1 = float(calibration.find('k1').text) if calibration.find('k1') is not None and calibration.find('k1').text else 0
            k2 = float(calibration.find('k2').text) if calibration.find('k2') is not None and calibration.find('k2').text else 0
            k3 = float(calibration.find('k3').text) if calibration.find('k3') is not None and calibration.find('k3').text else 0
            k4 = float(calibration.find('k4').text) if calibration.find('k4') is not None and calibration.find('k4').text else 0
            p1 = float(calibration.find('p1').text) if calibration.find('p1') is not None and calibration.find('p1').text else 0
            p2 = float(calibration.find('p2').text) if calibration.find('p2') is not None and calibration.find('p2').text else 0
            b1 = float(calibration.find('b1').text) if calibration.find('b1') is not None and calibration.find('b1').text else 0
            b2 = float(calibration.find('b2').text) if calibration.find('b2') is not None and calibration.find('b2').text else 0
            
            sensor_calibrations = {
                'f': f,
                'cx': cx,
                'cy': cy,
                'k1': k1,
                'k2': k2,
                'k3': k3,
                'k4': k4,
                'p1': p1,
                'p2': p2,
                'b1': b1,
                'b2': b2
            }
            print(f"Camera calibrations loaded successfully: {sensor_calibrations}")
        except Exception as e:
            print(f"Error processing calibration data: {e}")
            sensor_calibrations = {}
    if not sensor_calibrations:
        print("Warning: No valid camera calibrations found.")
    return sensor_calibrations

# Function to generate mask for given image
def generate_mask(coco, img_info, anns_class, anns_maskbound):
    img_width, img_height = img_info['width'], img_info['height']
    mask = np.zeros((img_height, img_width), dtype=np.uint8)
    
    class_priority = {'Wall': 1, 'Window': 2}
    class_index = {'Wall': 1, 'Window': 2}

    anns_combined = list(zip(anns_class, anns_maskbound))
    filtered_anns = [(cls, boundaries) for cls, boundaries in anns_combined if cls in class_priority]
    anns_sorted = sorted(filtered_anns, key=lambda x: class_priority[x[0]])

    for cls, boundaries in anns_sorted:
        for boundary in boundaries:
            pts = np.array(boundary, np.int32).reshape((-1, 1, 2))
            cv2.fillPoly(mask, [pts], class_index[cls])
    
    return mask

# Function to convert a 3D point to a 2D pixel
def convert_point_to_pixel(point, camera_info, camera_para, img_width, img_height, img_name):
    img_name_label = img_name.split('.')[0]
    cam = next((camera for camera in camera_info if camera['label'] == img_name_label), None)
    
    transform_list = list(map(float, cam['transform'].split()))
    transform_matrix = np.array(transform_list).reshape(4, 4)
    transform_matrix_3Dto2D = inv(transform_matrix)
    
    point_h = np.append(point, 1)
    point_2D = transform_matrix_3Dto2D @ point_h
    X, Y, Z = point_2D[0:3]
    x, y = X/Z, Y/Z

    f, cx, cy = camera_para['f'], camera_para['cx'], camera_para['cy']
    k1, k2, k3, k4 = camera_para['k1'], camera_para['k2'], camera_para['k3'], camera_para['k4']
    p1, p2, b1, b2 = camera_para['p1'], camera_para['p2'], camera_para['b1'], camera_para['b2']

    r = math.sqrt(x**2 + y**2)
    x_rad = x*(1 + k1*r**2 + k2*r**4 + k3*r**6 + k4*r**8)
    x_tang = (p1*(r**2 + 2*x**2) + 2*p2*x*y)
    y_rad = y*(1 + k1*r**2 + k2*r**4 + k3*r**6 + k4*r**8)
    y_tang = (p2*(r**2 + 2*y**2) + 2*p1*x*y)

    x_dash = x_rad + x_tang
    y_dash = y_rad + y_tang

    u = img_width * 0.5 + cx + x_dash * f + x_dash * b1 + y_dash * b2
    v = img_height * 0.5 + cy + y_dash * f
    
    return np.asarray([u, v])

# Function to project points to image with weighting
def project_points_to_image(point_cloud, camera_info, camera_para, img_width, img_height, img_name, mask):
    class_info_list = np.zeros(len(point_cloud), dtype=float)
    weight_sum_list = np.zeros(len(point_cloud), dtype=float)
    
    img_name_label = img_name.split('.')[0]
    cam = next((camera for camera in camera_info if camera['label'] == img_name_label), None)
    
    transform_list = list(map(float, cam['transform'].split()))
    transform_matrix = np.array(transform_list).reshape(4, 4)
    transform_matrix_3Dto2D = inv(transform_matrix)
    
    for i, point in enumerate(point_cloud):
        # Convert 3D point to 2D pixel
        point_h = np.append(point, 1)
        point_2D = transform_matrix_3Dto2D @ point_h
        X, Y, Z = point_2D[0:3]
        x, y = X/Z, Y/Z
        
        if Z <= 0:  # Ignore points behind the camera
            continue

        # Compute weight based on distance from camera
        weight = 1 / Z  # Weight inversely proportional to depth (closer points have higher weight)

        f, cx, cy = camera_para['f'], camera_para['cx'], camera_para['cy']
        k1, k2, k3, k4 = camera_para['k1'], camera_para['k2'], camera_para['k3'], camera_para['k4']
        p1, p2, b1, b2 = camera_para['p1'], camera_para['p2'], camera_para['b1'], camera_para['b2']

        r = math.sqrt(x**2 + y**2)
        x_rad = x*(1 + k1*r**2 + k2*r**4 + k3*r**6 + k4*r**8)
        x_tang = (p1*(r**2 + 2*x**2) + 2*p2*x*y)
        y_rad = y*(1 + k1*r**2 + k2*r**4 + k3*r**6 + k4*r**8)
        y_tang = (p2*(r**2 + 2*y**2) + 2*p1*x*y)

        x_dash = x_rad + x_tang
        y_dash = y_rad + y_tang

        u = img_width * 0.5 + cx + x_dash * f + x_dash * b1 + y_dash * b2
        v = img_height * 0.5 + cy + y_dash * f
        
        # Check if pixel is within image bounds
        if (0 <= u < img_width) and (0 <= v < img_height):
            class_info = mask[int(v), int(u)]
            if class_info > 0:  # Only consider valid classes
                class_info_list[i] += class_info * weight
                weight_sum_list[i] += weight

    # Normalize the class info by the sum of weights
    class_info_list /= np.where(weight_sum_list != 0, weight_sum_list, 1)
    
    # Round to nearest integer class label
    class_info_list = np.round(class_info_list).astype(int)
    
    return class_info_list

# Function to apply statistical outlier removal
def apply_statistical_outlier_removal(point_cloud, nb_neighbors=20, std_ratio=1.0):
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(point_cloud)
    cl, ind = pcd.remove_statistical_outlier(nb_neighbors, std_ratio)
    return np.asarray(pcd.points)

# Function to process multiple images and project them onto point cloud with weighting
def process_images_and_project_to_point_cloud(image_count, coco, camera_info, camera_para, point_cloud_file):
    imgIds = coco.getImgIds()[:image_count]

    # Read the point cloud
    pc = o3d.io.read_point_cloud(point_cloud_file)
    point_cloud = np.asarray(pc.points)

    # Apply outlier removal
    point_cloud = apply_statistical_outlier_removal(point_cloud)

    # Downsample for efficiency
    ds_points = point_cloud[::100]

    accumulated_class_info = np.zeros(len(ds_points), dtype=int)

    for img_id in imgIds:
        img_info = coco.loadImgs(img_id)[0]
        img_name = img_info['file_name']
        img_height = img_info['height']
        img_width = img_info['width']

        annIds = coco.getAnnIds(imgIds=img_id, iscrowd=None)
        anns = coco.loadAnns(annIds)
        anns_class = [coco.loadCats(ann['category_id'])[0]['name'] for ann in anns]
        anns_maskbound = [ann['segmentation'] for ann in anns]

        mask = generate_mask(coco, img_info, anns_class, anns_maskbound)
        class_info_list = project_points_to_image(ds_points, camera_info, camera_para, img_width, img_height, img_name, mask)

        accumulated_class_info = np.maximum(accumulated_class_info, class_info_list)

    color_map = {-1: [0.5, 0.5, 0.5], 1: [1.0, 0.0, 0.0], 2: [0.0, 0.0, 1.0]}  # Grey for unclassified, red for walls, blue for windows
    point_cloud_new = o3d.geometry.PointCloud()
    point_cloud_new.points = o3d.utility.Vector3dVector(ds_points)
    point_colors = [color_map.get(class_id, [0.5, 0.5, 0.5]) for class_id in accumulated_class_info]
    point_cloud_new.colors = o3d.utility.Vector3dVector(point_colors)

    o3d.visualization.draw_geometries([point_cloud_new])


# Initialize COCO API
annotation_file = './all_objects1.json'
coco = COCO(annotation_file)

# Read camera information
file_path = './camera2.xml'
camera_info = read_camera_transforms(file_path)
camera_para = read_camera_calibrations(file_path)

# Process all images and project them onto the point cloud
process_images_and_project_to_point_cloud(image_count=15, coco=coco, camera_info=camera_info, camera_para=camera_para, point_cloud_file='./room3.ply')




步骤 1: 读取相机信息和标定数据
首先，我们需要从XML文件中读取相机的变换矩阵和标定数据。这些信息将用于后续的投影和射线生成。
步骤 2: 生成Mask
使用COCO工具生成图像中wall类别的Mask。这个Mask将用于确定哪些像素属于wall类别。
步骤 3: 计算相机中心坐标
根据相机的变换矩阵计算相机中心的坐标。
步骤 4: 生成射线
对于每个属于wall类别的像素，生成从相机中心出发的射线。
步骤 5: 计算射线与平面的交点
计算射线与给定平面 0.793553x + -0.040436y + 0.607156z + -2.950997 = 0 的交点。
步骤 6: 融合新的点云
将计算得到的交点与原始点云融合，形成新的点云模型。
步骤 7: 可视化
使用Open3D可视化新的点云模型，并在原有的点云模型上标注出新点。