Azure Kinect双相机3D坐标系融合：点云对齐效果不佳排查

我需要对齐两台Azure Kinect采集的人体关节点云数据。校准流程为：受试者保持T姿静止15秒，每帧包含两个坐标系下的32个3D关节坐标（如camera1的LEFT_KNEE为(x1,y1,z1)，camera2的LEFT_KNEE为(x2,y2,z3)）。我的方案是计算校准阶段每帧的旋转矩阵R和平移向量T，取均值后用于同受试者同机位的其他数据集。目前代码可运行，能从3个视角可视化点云，但视觉上点云未正确对齐，手动调整旋转效果不佳，已排除数据镜像问题，尝试过Open3D但不理解内部逻辑，现求助排查：是代码错误还是数据本身存在问题？

附代码：

import json
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np


def calculate_transform_matrices(m_points_array, s_points_array):
    # centroids of the point sets
    centroid1 = np.mean(m_points_array, axis =0)
    centroid2 = np.mean(s_points_array, axis =0)

    centroid1 = m_points_array[0]
    centroid2 = s_points_array[0]

    # point sets centered around their centroids
    centered_m_points = m_points_array - centroid1
    centered_s_points = s_points_array - centroid2

    # Ccovariance matrix
    covariance_matrix = np.dot(centered_s_points.T, centered_m_points)

    # singular value decomposition (SVD)
    U, _, Vt = np.linalg.svd(covariance_matrix)

    # rotation matrix = R
    R = np.dot(Vt.T, U.T)

    # translation vector = T
    T = centroid1 - np.dot(R, centroid2)

    return R, T


JOINTANZAHL = 32
KNOCHENANZAHL = 33

#############################################################
first_sub = 24

last_sub = 27

start_frame = 150
last_frame = 300

m= "master"
s= "subord"

#####################################################################################
## open the need files from master and suprd
#####################################################################################   

for SUBJECT in range(first_sub, last_sub+1):
    m_file = "s"+str(SUBJECT)+"_calib_popup_"+str(m)+"_"
    s_file = "s"+str(SUBJECT)+"_calib_popup_"+str(s)+"_"

    with open(str(m_file)+".json",  'r') as f:
        m_dataset = f.read()
    
    with open(str(s_file)+".json",  'r') as f:
        s_dataset = f.read()

    m_obj = json.loads(m_dataset)
    s_obj = json.loads(s_dataset)

#####################################################################################
## Calculating the R and T for each frame --> then calculate the mean frame out of all frame Rs and Ts
#####################################################################################   
    

    rotation_matrices = []
    translation_vectors = []

    for frame in range(start_frame,last_frame):
        m_points_lst = []
        s_points_lst = []
        
        for joint in range (JOINTANZAHL):
            m_points = m_obj['frames'][frame]['bodies'][0]['joint_positions'][joint]
            s_points = s_obj['frames'][frame]['bodies'][0]['joint_positions'][joint]


            m_points_lst.append(m_points)
            s_points_lst.append(s_points)

        m_points_array = np.array(m_points_lst)
        s_points_array = np.array(s_points_lst)

        R, T = calculate_transform_matrices(m_points_array, s_points_array)
        
        rotation_matrices.append(R)
        translation_vectors.append(T)
               
    mean_R = np.mean(rotation_matrices, axis=0)
    mean_T = np.mean(translation_vectors, axis=0)

#####################################################################################
## Use the mean R and T to transform the original into the aligned data (master & subord)
#####################################################################################   
    
    ## aligned point clouds 
    subord_aligned_joint_positions = []
    master_aligned_joint_positions = []

    for frame in range(start_frame, last_frame):
        m_points_lst = []
        s_points_lst = []

        for joint in range (JOINTANZAHL):
            m_points = m_obj['frames'][frame]['bodies'][0]['joint_positions'][joint]
            s_points = s_obj['frames'][frame]['bodies'][0]['joint_positions'][joint]

            m_points_lst.append(m_points)
            s_points_lst.append(s_points)

        m_points_array = np.array(m_points_lst)
        s_points_array = np.array(s_points_lst)
        
        R = mean_R
        T = mean_T
        
          
        
        aligned_s_points = np.dot(s_points_array, R.T) + T
        
        ## possibilty to change/adjust the R of one point cloud 
        #R = np.dot(R, np.array([[-1, 0, 0], [0, 1, 0], [-0.9, 0, 1]]))
      
        aligned_m_points = np.dot(m_points_array, R.T) + T
  
        subord_aligned_joint_positions.append(aligned_s_points)
        master_aligned_joint_positions.append(aligned_m_points)
    
    ## here we print one example point e.g. frame 9 = hand 
    print("sub: first frame, joint9 : "+str(subord_aligned_joint_positions[0][8]))
    print("master: first frame, joint9 : "+str(master_aligned_joint_positions[0][8]))
    

#####################################################################################
## Calculating the mean error --> how precise the data were aligned
#####################################################################################  
    

    errors = []
    
    for frame in range(len(subord_aligned_joint_positions)):
        for joint in range(JOINTANZAHL):
            subord_joint_pos = subord_aligned_joint_positions[frame][joint]
            master_joint_pos = master_aligned_joint_positions[frame][joint]
            
            error = np.sqrt(np.mean((subord_joint_pos - master_joint_pos)**2))
            errors.append(error)

    rmse = np.mean(errors)
    print("the RMSE is: "+str(rmse))
    

#####################################################################################
## Plot in three different point of views
#####################################################################################
    
    angles = [(0, 90), (0, 0), (90, 0)]

    visualization_frames = [1]

    fig, axes = plt.subplots(1, 3, figsize=(15, 10), dpi=200, subplot_kw={'projection': '3d'})
    for i, ax in enumerate(axes):
        elevation_angle, azimuth_angle = angles[i]
        
        for frame in visualization_frames:
            for joint in range(JOINTANZAHL):
                subord_joint_pos = subord_aligned_joint_positions[frame][joint]
                master_joint_pos = master_aligned_joint_positions[frame][joint]

                ax.scatter(subord_joint_pos[0], subord_joint_pos[1], subord_joint_pos[2], c='r', marker='o')
                ax.scatter(master_joint_pos[0], master_joint_pos[1], master_joint_pos[2], c='b', marker='o')

                ax.text(subord_joint_pos[0], subord_joint_pos[1], subord_joint_pos[2], str(joint), color='r')
                ax.text(master_joint_pos[0], master_joint_pos[1], master_joint_pos[2], str(joint), color='b')

        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.set_zlabel('Z')
        ax.view_init(elevation_angle, azimuth_angle)
        
    fig.text(0.5, 0.2, "RMS = "+str(rmse), ha='center')
    fig.text(0.5, 0.17, "translation", ha='center')
    fig.text(0.5, 0.15, str(T), ha='center')
    fig.text(0.5, 0.11, "rotation", ha='center')
    fig.text(0.5, 0.05, str(R), ha='center')
    
    plt.suptitle("s"+str(SUBJECT)+" calibration - Frame: "+str(visualization_frames), fontsize=16)
    
    plt.tight_layout()
    plt.show()

排查结果：代码存在多处关键错误 #

1. 质心计算被错误覆盖 #

在calculate_transform_matrices函数中，先通过np.mean计算了正确的点集质心，但随后用第一个关节点m_points_array[0]和s_points_array[0]覆盖了质心值，完全违背ICP算法基于点集整体质心对齐的核心逻辑，直接导致旋转和平移计算严重偏差。

2. 变换方向错误 #

ICP算法中，R和T的作用是将源点集（subord）变换到目标点集（master）的坐标系。当前代码错误地对master点云也应用了变换，正确做法是仅变换subord点云，master点云保持原坐标系不变，才能有效对比对齐效果。

3. 旋转矩阵未做正交校验 #

SVD计算得到的旋转矩阵可能出现行列式为-1的反射变换（而非纯旋转），需要添加校验修正：

# 在计算R后添加
det = np.linalg.det(R)
if det < 0:
    Vt[-1, :] *= -1
    R = np.dot(Vt.T, U.T)

4. RMSE计算逻辑错误 #

当前计算的是单个关节的均方根误差再取均值，正确的RMSE应该是所有关节点对的误差平方和取均值后开根号：

errors = []
for frame in range(len(subord_aligned_joint_positions)):
    frame_error = np.sum((subord_aligned_joint_positions[frame] - m_points_array)**2)
    errors.append(np.sqrt(frame_error / JOINTANZAHL))
rmse = np.mean(errors)

5. 可视化帧索引混淆 #

visualization_frames = [1]对应的是校准阶段的第151帧（因为start_frame=150），若要查看校准第一帧，应改为[0]。

数据层面补充排查建议 #

• 确认两台相机的关节点顺序完全对应：比如joint9在两个数据集中确实是同一人体部位，关节顺序错位会直接导致对齐失败。
• 检查校准帧稳定性：确认start_frame到last_frame之间受试者保持T姿无明显晃动，若存在运动，单帧R/T波动会导致均值失效。
• 验证坐标系一致性：打印关键关节的相对位置（如左肩-右肩的X轴方向），确认两台相机的坐标系规则统一。

内容的提问来源于stack exchange，提问作者user20866357