大疆无人机M4T拍照定位

获取大疆无人机拍照图像，通过飞行遥测和相机参数，计算任意像素坐标下对应的地理位置坐标；

在激光测距有效的情况下，云台垂直向下时，定位误差不超过3m。

通过在线exif工具可以解析图像参数EXIF在线工具

获取参数后，先对像素坐标转相机坐标系

cpp 复制代码

std::vector<double> DroneTargetLocator::pixelToCameraNormalized(
    double px, double py) const {
    // 计算有效焦距（考虑电子变倍）
    // 焦距单位：毫米（mm）
    double effective_focal = focal_length_ * digital_zoom_;
    
    // 计算像素到物理尺寸的转换比例
    // sensor_width_ 和 sensor_height_ 单位：毫米（mm）
    // image_width_ 和 image_height_ 单位：像素
    // pixel_size_x/y 单位：毫米/像素（mm/pixel）
    double pixel_size_x = sensor_width_ / static_cast<double>(image_width_);
    double pixel_size_y = sensor_height_ / static_cast<double>(image_height_);
    
    // 将像素坐标转换为相对于图像中心的坐标（像素单位）
    double cx = px - image_width_ / 2.0;
    double cy = image_height_ / 2.0 - py;  // Y轴翻转（图像坐标系Y向下，相机坐标系Y向上）
    
    // 转换为物理尺寸（毫米）
    // cx * pixel_size_x 单位：像素 * (mm/pixel) = mm
    // cy * pixel_size_y 单位：像素 * (mm/pixel) = mm
    double x_mm = cx * pixel_size_x;
    double y_mm = cy * pixel_size_y;
    
    // 归一化到相机坐标系
    // 大疆相机坐标系：X右，Y上，Z前（光轴方向）
    // 归一化向量：相机坐标系中指向目标点的单位方向向量
    // x_mm / effective_focal 单位：mm / mm = 无量纲
    // y_mm / effective_focal 单位：mm / mm = 无量纲
    double x_norm = x_mm / effective_focal;
    double y_norm = y_mm / effective_focal;
    double z_norm = 1.0;  // 焦距归一化
    
    // 归一化为单位向量
    double norm = std::sqrt(x_norm * x_norm + y_norm * y_norm + z_norm * z_norm);
    if (norm < 1e-10) {
        // 避免除零，如果norm太小，返回光轴方向
        return {0.0, 0.0, 1.0};
    }
    return {x_norm / norm, y_norm / norm, z_norm / norm};
}

如果相机安装和云台保持一致，不需要考虑转换。

将云台坐标系转NED坐标系，部分代码如下：

cpp 复制代码

// 绝对角模式：云台角度直接相对于NED坐标系（地理坐标系）
        // 大疆无人机角度定义：
        // - 俯仰角：垂直向下为-90°，水平为0°，向上为正
        // - 滚转角：右滚为正
        // - 方位角：北为0°，顺时针为正
        // 相机坐标系：X右，Y上，Z前（光轴）
        // NED坐标系：X北，Y东，Z地（向下）
        
        // 大疆绝对角模式下的转换逻辑：
        // 1. 当pitch=0, roll=0, yaw=0时，相机Z前应指向NED X北（水平向前）
        // 2. 当pitch=-90°, roll=0, yaw=0时，相机Z前应指向NED Z地（垂直向下）
        // 3. 当pitch=0, roll=0, yaw=90°时，相机Z前应指向NED Y东（水平向右）
        
        // 基础对齐矩阵：将相机坐标系对齐到NED坐标系（当所有角度为0时）
        // 相机Z前 -> NED X北，相机X右 -> NED Y东，相机Y上 -> NED Z地（取反）
        std::vector<std::vector<double>> R_base(3, std::vector<double>(3));
        R_base[0][0] = 0.0; R_base[0][1] = 0.0; R_base[0][2] = 1.0;   // N = 相机Z（前）
        R_base[1][0] = 1.0; R_base[1][1] = 0.0; R_base[1][2] = 0.0;   // E = 相机X（右）
        R_base[2][0] = 0.0; R_base[2][1] = -1.0; R_base[2][2] = 0.0;  // D = -相机Y（上，取反因为NED的Z向下）

最后NED转换为地理坐标，（部分代码如下）

cpp 复制代码

/ NED坐标系：N（北）、E（东）、D（地/下）
    // 将NED向量转换为地理坐标
    
    // ned_vec是单位方向向量，需要根据实际距离计算NED坐标
    // 计算方向向量的分量
    double dir_north = ned_vec[0];
    double dir_east = ned_vec[1];
    double dir_down = ned_vec[2];
    
    // 检查方向向量是否有效
    double norm = std::sqrt(dir_north * dir_north + dir_east * dir_east + dir_down * dir_down);
    if (norm < 1e-6) {
        return GeoCoordinate(drone_longitude_, drone_latitude_, drone_altitude_);
    }
    
    // 归一化方向向量
    dir_north /= norm;
    dir_east /= norm;
    dir_down /= norm;
    
    // 根据相机视线方向计算实际距离
    // 假设目标在地面上（高度为0），根据高度差和视线方向计算距离
    double distance = 0.0;
    
    // 假设目标高度为0（地面），或可以根据实际情况调整
    double target_height = 4.32;  // 目标高度（米），可以根据实际情况修改
    double height_diff = drone_altitude_ - target_height;

如果激光测距有效，可以使用激光测距进行修正。