# drone_coord

**Repository Path**: bestwsy/drone_coord

## Basic Information

- **Project Name**: drone_coord
- **Description**: 无人机识别物体定位目标的坐标变换模块
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 1
- **Created**: 2025-08-25
- **Last Updated**: 2025-08-25

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 多视角协同定位
- 双车联合定位
- 无人机定位
- 融合双车和无人机定位
  
## 无人机定位

- 使用ENU坐标系
- 获取GPS数据
- 根据相机识别到物体的像素定位目标位置

### ENU坐标系
- X轴：东方向
- Y轴：北方向
- Z轴：垂直向上

### 坐标转换流程
1. **像素坐标系 → 相机坐标系**
   - 输入：像素坐标 (pixel.x, pixel.y)
   - 输出：相机坐标系下的单位向量 camera_vec

2. **相机坐标系 → 机体坐标系**
   - 输入：相机坐标系向量 camera_vec 和相机安装旋转矩阵 camera_rot
   - 输出：机体坐标系下的向量 body_vec

3. **机体坐标系 → 全局坐标系**
   - 输入：机体坐标系向量 body_vec 和无人机姿态旋转矩阵 drone_rot
   - 输出：全局坐标系下的射线方向 global_vec

4. **射线与地面相交计算**
   - 输入：无人机位置 global_pos 和射线方向 global_vec
   - 输出：地面目标坐标 result

### 数学推导
#### 像素点到相机坐标系射线
$$
\text{齐次坐标} = K^{-1} \cdot [u, v, 1]^T
$$
$$
\text{单位方向向量} = \frac{\text{齐次坐标}}{\|\text{齐次坐标}\|}
$$

#### 考虑相机姿态
假设相机光轴与无人机“北”方向呈45度角，则旋转矩阵 $R$ 可能为：
$$
R = \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos(45^\circ) & -\sin(45^\circ) \\
0 & \sin(45^\circ) & \cos(45^\circ)
\end{bmatrix}
$$

#### 射线与地面交点
设无人机当前在ENU坐标系中的位置为 $(x_0, y_0, h)$，射线方向为 $\vec{d} = (d_x, d_y, d_z)$，则射线方程为：
$$
(x, y, z) = (x_0, y_0, h) + t \cdot (d_x, d_y, d_z)
$$
求解 $z = 0$ 时的 $t$：
$$
h + t \cdot d_z = 0 \Rightarrow t = -\frac{h}{d_z}
$$
代入 $t$ 得到交点：
$$
(x, y, 0) = (x_0 - \frac{h}{d_z} \cdot d_x, y_0 - \frac{h}{d_z} \cdot d_y, 0)
$$

#### 最终结果
目标物体的ENU坐标为：
$$
x_{\text{target}} = x_0 - \frac{h}{d_z} \cdot d_x \\
y_{\text{target}} = y_0 - \frac{h}{d_z} \cdot d_y
$$