根据无人机gps和相应朝向信息计算所拍摄图片像素点的经纬度cpp代码
时间: 2024-02-05 18:12:11 浏览: 37
以下是计算无人机拍摄图片像素点经纬度的 C++ 代码:
```cpp
#include <iostream>
#include <cmath>
const double PI = 3.1415926535898;
const double EARTH_RADIUS = 6378137.0; // 地球半径,单位米
// 计算经度差
double calcLonDiff(double lat, double distance) {
double dlon = 2 * asin(sin(distance / (2 * EARTH_RADIUS)) / cos(lat));
return dlon;
}
// 计算纬度差
double calcLatDiff(double distance) {
double dlat = distance / EARTH_RADIUS;
return dlat;
}
// 计算像素点经纬度
void calcPixelGPS(double droneLat, double droneLon, double droneYaw,
double pixelX, double pixelY, double imgWidth, double imgHeight, double fov,
double &pixelLat, double &pixelLon) {
double fovRad = fov * PI / 180.0; // 视角弧度值
double f = imgWidth / (2.0 * tan(fovRad / 2.0)); // 焦距
double pixelXm = (pixelX - imgWidth / 2.0) / f; // 计算像素点到光心的距离(以焦距为单位)
double pixelYm = (imgHeight / 2.0 - pixelY) / f;
double pixelDistance = sqrt(pixelXm * pixelXm + pixelYm * pixelYm + 1.0); // 像素点到光心的距离(单位为焦距)
double pixelYaw = atan2(pixelXm, 1.0); // 像素点的航向角(与正北方向的夹角)
double droneYawRad = droneYaw * PI / 180.0; // 无人机朝向弧度值
double heading = pixelYaw + droneYawRad; // 像素点的航向角(与正北方向的夹角)
double pixelDistanceReal = pixelDistance * cos(droneYawRad) * cos(pixelYaw)
- sin(droneYawRad) * sin(pixelYaw); // 像素点到无人机的实际距离
double latDiff = calcLatDiff(pixelDistanceReal); // 计算纬度差
double lonDiff = calcLonDiff(droneLat * PI / 180.0, pixelDistanceReal); // 计算经度差
pixelLat = droneLat + latDiff * 180.0 / PI; // 像素点的纬度
pixelLon = droneLon + lonDiff * 180.0 / PI; // 像素点的经度
}
int main() {
double droneLat = 30.0; // 无人机当前位置纬度
double droneLon = 120.0; // 无人机当前位置经度
double droneYaw = 90.0; // 无人机当前朝向,以正北方向为0度,顺时针旋转为正值
double pixelX = 100.0; // 图片像素点横坐标
double pixelY = 200.0; // 图片像素点纵坐标
double imgWidth = 1920.0; // 图片宽度
double imgHeight = 1080.0; // 图片高度
double fov = 85.0; // 图片视角,单位度
double pixelLat, pixelLon; // 像素点经纬度
calcPixelGPS(droneLat, droneLon, droneYaw, pixelX, pixelY, imgWidth, imgHeight, fov, pixelLat, pixelLon);
std::cout << "Pixel GPS: " << pixelLat << ", " << pixelLon << std::endl;
return 0;
}
```
需要注意的是,该代码中假设地球为标准的球体,实际应用时可能需要考虑地球的椭球形。另外,该代码中假设图片中心为光心,实际应用时需要根据相机参数进行调整。