2.7 KiB
2.7 KiB
RSEM4 angle and XYZ formulas
本文只说明 RsLidarDevice 中 RSEM4 雷达角度、距离与
PointCloudCallback 输出 XYZ 之间的正算和逆算公式。
代码依据：Src/RsLidarDevice.cpp::fillRsem4Point()。
单位约定
- 原始距离分辨率：
1 unit = 0.005 m。 - 公式中的
distance_m单位为米。 PointCloudCallback输出的X/Y/Z单位为毫米。- 代码中的角度单位为
0.01°，例如1300 = 13.00°。 - 三角函数计算时需要先把
0.01°转成弧度：
angle_rad = angle_deg01 * pi / 18000
角度定义
水平角：
yaw_deg01 = yawBase_deg01 + yawOffset_deg01[ring / 20]
垂直角：
pitch_deg01 = pitchAngle_deg01[ring] + surfacePitchOffset_deg01[surfaceIndex]
转换成弧度：
yaw_rad = yaw_deg01 * pi / 18000
pitch_rad = pitch_deg01 * pi / 18000
正算公式：angle + distance -> XYZ
雷达内部先按标准球坐标计算：
x0_m = distance_m * cos(pitch_rad) * cos(yaw_rad)
y0_m = distance_m * cos(pitch_rad) * sin(yaw_rad)
z0_m = distance_m * sin(pitch_rad)
再转换成 PointCloudCallback 输出坐标：
X_mm = -y0_m * 1000
Y_mm = -z0_m * 1000
Z_mm = x0_m * 1000
合并后：
X_mm = -distance_m * cos(pitch_rad) * sin(yaw_rad) * 1000
Y_mm = -distance_m * sin(pitch_rad) * 1000
Z_mm = distance_m * cos(pitch_rad) * cos(yaw_rad) * 1000
强度值：
intensity = 原始 intensity
逆算公式：XYZ -> angle + distance
给定 PointCloudCallback 输出点：
X_mm = point.x
Y_mm = point.y
Z_mm = point.z
距离：
range_mm = sqrt(X_mm * X_mm + Y_mm * Y_mm + Z_mm * Z_mm)
range_m = range_mm / 1000
水平角：
yaw_rad = atan2(-X_mm, Z_mm)
yaw_deg = yaw_rad * 180 / pi
如果需要归一化到 [0, 360)：
if yaw_deg < 0:
yaw_deg += 360
垂直角：
pitch_rad = asin(-Y_mm / range_mm)
pitch_deg = pitch_rad * 180 / pi
转换成代码中的 0.01° 单位：
yaw_deg01 = yaw_deg * 100
pitch_deg01 = pitch_deg * 100
反推原始角度
上面的逆算得到的是已经叠加 DIFOP 标定补偿后的实际出射角。
如果已知当前点的 ring、surfaceIndex 和 DIFOP 标定表，则可继续反推：
yawBase_deg01 = yaw_deg01 - yawOffset_deg01[ring / 20]
pitchAngle_deg01[ring] = pitch_deg01 - surfacePitchOffset_deg01[surfaceIndex]
如果只拿到 XYZ，不知道 ring、surfaceIndex 和 DIFOP 标定表，则不能唯一反推原始包里的
yawAngle、pitchAngle 或 surfaceId。
无效点
当输出点为零点时：
X_mm = 0
Y_mm = 0
Z_mm = 0
不能进行角度逆算。零点可能来自距离越界或 surfaceIndex 非法。