Intel RealSense D435倾斜相机的目标相对高度计算问题

你的问题核心是像素转相机坐标系时的高度轴符号错误，导致旋转后的世界坐标系高度计算出现近高远低的偏差，同时需要确认旋转角度的方向是否匹配实际场景。

1. 修正像素到相机坐标系的高度计算 #

RealSense的像素坐标系默认以左上角为原点，y轴向下延伸。你定义z为高度轴（向上为正），但原始代码直接用(y - self.Cy) * coords.x * invFy计算z值，会出现符号颠倒的问题：

• 当目标在图像中高于相机中心（y < Cy），y-Cy为负，计算出的z为负（实际应为正高度）
• 当目标在图像中低于相机中心（y > Cy），计算出的z为正（实际应为负高度）

修正后的坐标转换代码：

def findPixelCoords(self, x: float, y: float, d: Image) -> Point:
        coords: Point = Point()

        invFx = 1/self.Fx
        invFy = 1/self.Fy

        coords.x = d[y][x] * 0.001  # 相机前向深度（原定义的x轴）
        coords.y = ((x - self.Cx) * coords.x * invFx) + 0.037  # 水平轴（原定义的y轴）
        # 修正高度轴符号：y像素向下为正，向上高度取负
        coords.z = -(y - self.Cy) * coords.x * invFy
        
        return coords

2. 确认旋转角度的方向与公式匹配 #

你使用的绕y轴旋转公式本身是正确的，但需要确保theta的正负与相机倾斜方向一致：

• 若相机向下倾斜30°，theta = math.radians(30)
• 若相机向上倾斜30°，theta = math.radians(-30)

修正后的旋转代码（结合修正后的坐标）：

import math

theta = math.radians(30)  # 根据实际倾斜方向调整正负
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)

# 原始相机坐标系坐标
oldX = coords.x
oldZ = coords.z

# 转换到世界坐标系
newX = oldX * cos_theta + oldZ * sin_theta
newZ = -oldX * sin_theta + oldZ * cos_theta

3. 问题根源解释 #

之前的newZ异常完全源于高度轴符号颠倒：

• 近距离目标：原始z值符号错误，导致旋转时oldZ的贡献方向相反，计算出的newZ偏高
• 远距离目标：原始z值的符号错误被-oldX*sin(theta)项放大，导致newZ偏低

修正后，旋转公式会正确映射相机坐标系下的高度到世界坐标系，同时保持不同距离下的实际高度差恒定。

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