奥比中光的dabai_dcw2相机彩色对齐方案

方法1：通过CloudCompare手动计算（推荐初学者）
​​在CloudCompare中加载PCD文件​​
打开A/B位置的PCD文件，点云已包含RGB颜色信息。
​​定位红点​​
使用“剪刀工具”框选红点区域，软件会显示选中点的平均坐标（X,Y,Z）。
​​定位地面参考点​​
在红点附近选择至少3个地面点（避开雪糕筒）。
​​拟合地面平面​​
选择地面点 → 点击“Tools” → “Fit” → “Plane”。软件会生成拟合平面方程（如 ax+by+cz+d=0）。
​​计算高度​​
红点高度 = 红点Z坐标 - 拟合平面对应的Z值（或直接使用点到平面的距离公式）。
方法2：通过投影矩阵自动计算（更精确）
使用同事提供的投影矩阵（P），将红点从相机坐标系直接转换到世界坐标系：

import numpy as np

定义投影矩阵 P（同事提供）

P = np.array([
[-455.14, 253.33, 246.40, 205.78],
[5.37, -175.68, 504.92, 223.75],
[0.07, 0.69, 0.72, 0.65]
])

假设红点在相机坐标系下的坐标（来自点云）

point_camera = np.array([x_cam, y_cam, z_cam, 1]) # 齐次坐标

转换到世界坐标系

point_world = P @ point_camera # 矩阵乘法
height = point_world[2] # 世界坐标系的Z值即高度
​​说明​​：

(x_cam, y_cam, z_cam) 是点云中红点的相机坐标系坐标（通过CloudCompare或ROS读取）。
​​结果解读​​：height 直接是红点相对于棋盘格平面的高度，无需额外拟合地面。