143 lines
2.7 KiB
Markdown
143 lines
2.7 KiB
Markdown
|
# 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` 非法。
|