# 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°` 转成弧度：

```text
angle_rad = angle_deg01 * pi / 18000
```

## 角度定义

水平角：

```text
yaw_deg01 = yawBase_deg01 + yawOffset_deg01[ring / 20]
```

垂直角：

```text
pitch_deg01 = pitchAngle_deg01[ring] + surfacePitchOffset_deg01[surfaceIndex]
```

转换成弧度：

```text
yaw_rad   = yaw_deg01 * pi / 18000
pitch_rad = pitch_deg01 * pi / 18000
```

## 正算公式：angle + distance -> XYZ

雷达内部先按标准球坐标计算：

```text
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` 输出坐标：

```text
X_mm = -y0_m * 1000
Y_mm = -z0_m * 1000
Z_mm =  x0_m * 1000
```

合并后：

```text
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
```

强度值：

```text
intensity = 原始 intensity
```

## 逆算公式：XYZ -> angle + distance

给定 `PointCloudCallback` 输出点：

```text
X_mm = point.x
Y_mm = point.y
Z_mm = point.z
```

距离：

```text
range_mm = sqrt(X_mm * X_mm + Y_mm * Y_mm + Z_mm * Z_mm)
range_m  = range_mm / 1000
```

水平角：

```text
yaw_rad = atan2(-X_mm, Z_mm)
yaw_deg = yaw_rad * 180 / pi
```

如果需要归一化到 `[0, 360)`：

```text
if yaw_deg < 0:
    yaw_deg += 360
```

垂直角：

```text
pitch_rad = asin(-Y_mm / range_mm)
pitch_deg = pitch_rad * 180 / pi
```

转换成代码中的 `0.01°` 单位：

```text
yaw_deg01   = yaw_deg * 100
pitch_deg01 = pitch_deg * 100
```

## 反推原始角度

上面的逆算得到的是已经叠加 DIFOP 标定补偿后的实际出射角。

如果已知当前点的 `ring`、`surfaceIndex` 和 DIFOP 标定表，则可继续反推：

```text
yawBase_deg01 = yaw_deg01 - yawOffset_deg01[ring / 20]
pitchAngle_deg01[ring] = pitch_deg01 - surfacePitchOffset_deg01[surfaceIndex]
```

如果只拿到 XYZ，不知道 `ring`、`surfaceIndex` 和 DIFOP 标定表，则不能唯一反推原始包里的
`yawAngle`、`pitchAngle` 或 `surfaceId`。

## 无效点

当输出点为零点时：

```text
X_mm = 0
Y_mm = 0
Z_mm = 0
```

不能进行角度逆算。零点可能来自距离越界或 `surfaceIndex` 非法。