无人机对地面运动目标定位---获取目标的移动方向和速度

目录

一、引子

我们利用单目无人机通过等时间间隔拍照的形式对地面某移动目标进行定位,当前,我们已经获得了每张相片上该目标的三维坐标,并且知道该无人机在飞行过程中拍照的时间间隔,那么我们就可以通过一定的计算,得到目标的运动方向和运动速度。

二、代码解释

1.导入的数据是由相片名称及目标点的三维数据构成的txt文件,所以我们需要建立字符串分割函数,获取txt文件内的数据。

2.定义圆周率数值

3.定义方向计算函数

4.从txt文件提取所需信息

5.计算目标在相邻相片间的方向变化角

6.计算目标在相邻相片间的移动距离及运动速度

三、完整代码展示

四、结果展示

本文所有代码均由CSDN用户CV-X.WANG提供,任何个人或者团体,不得进行商用和教学活动,引用或部分引用,均需获得授权。


一、引子

我们利用单目无人机通过等时间间隔拍照的形式对地面某移动目标进行定位,当前,我们已经获得了每张相片上该目标的三维坐标,并且知道该无人机在飞行过程中拍照的时间间隔,那么我们就可以通过一定的计算,得到目标的运动方向和运动速度。

二、代码解释

1.导入的数据是由相片名称及目标点的三维数据构成的txt文件,所以我们需要建立字符串分割函数,获取txt文件内的数据。

cpp 复制代码
//字符串分割
vector<string> split(const string &s, char delimiter) {
	vector<string> tokens;
	string token;
	istringstream tokenStream(s);
	while (getline(tokenStream, token, delimiter)) {
		tokens.push_back(token);
	}
	return tokens;
}

2.定义圆周率数值

cpp 复制代码
#define M_PI       3.14159265358979323846   // pi

3.定义方向计算函数

为获取目标在平面方向的移动方向,本文采用了在军事领域常见的360°方向法。即以正北为0°方向,顺时针方向为0-360°,比如说,正东方向:在我们的方向系统中即为90°方向。

这其中,

double lon1_rad = lon1 * M_PI / 180.0;

double lat1_rad = lat1 * M_PI / 180.0;

double lon2_rad = lon2 * M_PI / 180.0;

double lat2_rad = lat2 * M_PI / 180.0;

为弧度制。

cpp 复制代码
//方向函数
double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
	// Convert degrees to radians
	double lon1_rad = lon1 * M_PI / 180.0;
	double lat1_rad = lat1 * M_PI / 180.0;
	double lon2_rad = lon2 * M_PI / 180.0;
	double lat2_rad = lat2 * M_PI / 180.0;

	// Calculate delta longitude and convert to radians
	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;

	// Calculate y and x components
	double y = sin(delta_lon_rad) * cos(lat2_rad);
	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);

	// Calculate direction angle in radians
	double direction_rad = atan2(y, x);

	// Convert direction angle to degrees
	double direction_deg = direction_rad * 180.0 / M_PI;

	// Ensure direction angle is within [0, 360) degrees
	if (direction_deg < 0) {
		direction_deg += 360.0;
	}

	return direction_deg;
}

4.从txt文件提取所需信息

cpp 复制代码
	ifstream file("LBH.txt");
	if (!file.is_open()) {
		cerr << "Could not open the file!" << endl;
		return 1;
	}

	string line;
	// Skip the header line
	getline(file, line);

	vector<vector<string>> extractedData;

	// Read each line from the file
	while (getline(file, line)) {
		vector<string> columns = split(line, '\t');
		if (columns.size() < 16) {
			cerr << "Invalid line format" << endl;
			continue;
		}

		// Extract the required columns: 0, 13, 14, 15
		vector<string> extractedColumns;
		extractedColumns.push_back(columns[0]);  // Image Name
		extractedColumns.push_back(columns[13]); // Longitude
		extractedColumns.push_back(columns[14]); // Latitude
		extractedColumns.push_back(columns[15]); // Altitude

		extractedData.push_back(extractedColumns);
	}

	file.close();

5.计算目标在相邻相片间的方向变化角

cpp 复制代码
cout << "Direction angles between adjacent image centers:" << endl;
	for (size_t i = 1; i < extractedData.size(); ++i) {
		//三角函数计算用弧度制
		double lon1 = (stod(extractedData[i - 1][1]))* M_PI/180; // Longitude 
		double lat1 = (stod(extractedData[i - 1][2]))* M_PI / 180; // Latitude 
		double lon2 = (stod(extractedData[i][1]))* M_PI / 180;   // Longitude 
		double lat2 = (stod(extractedData[i][2]))* M_PI / 180;   // Latitude 
		//计算方向变化角也要用弧度制
		double direction_angle = calculateDirectionAngle(lon1, lat1, lon2, lat2);
		cout << "lon1=" << lon1 << endl << "lat1=" << lat1 << endl << "lon2=" << lon2 << endl << "lat2=" << lat2 << endl;
		// Output Direction
		cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << direction_angle << " degrees" << endl;

6.计算目标在相邻相片间的移动距离及运动速度

请注意:此处我们获得距离的计算式为:

这只是最简单的一个演示,实际情况下,我们需要考虑坐标系统、测区位置等等一系列的条件,从而获得更为精准的Distance。

cpp 复制代码
double lon2_1 = lon2 - lon1;
		double lat2_1 = lat2 - lat1;
		double lon_ = lon2_1 / 2;//1/2的Δlon
		double lat_ = lat2_1 / 2; //1 / 2的Δlat
		double sin2lon_ = sin(lon_)*sin(lon_);//sin²(1/2Δlon)
		double sin2lat_ = sin(lat_)*sin(lat_); //sin²(1 / 2Δlat)
		double cos_lat1 = cos(lat1);
		double cos_lat2 = cos(lat2);
		double sqrtA = sqrt(sin2lat_+ cos_lat1* cos_lat2*sin2lon_);
		//cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "sqrtA =" << sqrtA << endl;
		double asinA = asin(sqrtA);
		//长半轴 短半轴  单位是m
		int a_r = 6378137.0;
		int b_r = 6356752;
		double Earth_R = (2 * a_r + b_r) / 3;
		double Distance = 2 * Earth_R*asinA;
		cout << "Distance From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << Distance <<" meter"<< endl;
		int time = 3;//拍照间隔 s
		double speed = Distance / time;
		cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;
	}

三、完整代码展示

cpp 复制代码
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

using namespace std;
#define M_PI       3.14159265358979323846   // pi
// Function to split a string by a delimiter
vector<string> split(const string &s, char delimiter) {
	vector<string> tokens;
	string token;
	istringstream tokenStream(s);
	while (getline(tokenStream, token, delimiter)) {
		tokens.push_back(token);
	}
	return tokens;
}

//  direction angle in degrees
//原理是 在平面上以正北方向为0°方向,顺时针为0-360°
double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
	// Convert degrees to radians
	double lon1_rad = lon1 * M_PI / 180.0;
	double lat1_rad = lat1 * M_PI / 180.0;
	double lon2_rad = lon2 * M_PI / 180.0;
	double lat2_rad = lat2 * M_PI / 180.0;

	// Calculate delta longitude and convert to radians
	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;

	// Calculate y and x components
	double y = sin(delta_lon_rad) * cos(lat2_rad);
	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);

	// Calculate direction angle in radians
	double direction_rad = atan2(y, x);

	// Convert direction angle to degrees
	double direction_deg = direction_rad * 180.0 / M_PI;

	// Ensure direction angle is within [0, 360) degrees
	if (direction_deg < 0) {
		direction_deg += 360.0;
	}

	return direction_deg;
}

int main() {
	ifstream file("LBH.txt");
	if (!file.is_open()) {
		cerr << "Could not open the file!" << endl;
		return 1;
	}

	string line;
	// Skip the header line
	getline(file, line);

	vector<vector<string>> extractedData;

	// Read each line from the file
	while (getline(file, line)) {
		vector<string> columns = split(line, '\t');
		if (columns.size() < 16) {
			cerr << "Invalid line format" << endl;
			continue;
		}

		// Extract the required columns: 0, 13, 14, 15
		vector<string> extractedColumns;
		extractedColumns.push_back(columns[0]);  // Image Name
		extractedColumns.push_back(columns[13]); // Longitude
		extractedColumns.push_back(columns[14]); // Latitude
		extractedColumns.push_back(columns[15]); // Altitude

		extractedData.push_back(extractedColumns);
	}

	file.close();
	
	// Calculate direction angles between adjacent image centers
	cout << "Direction angles between adjacent image centers:" << endl;
	for (size_t i = 1; i < extractedData.size(); ++i) {
		//三角函数计算用弧度制
		double lon1 = (stod(extractedData[i - 1][1]))* M_PI/180; // Longitude 
		double lat1 = (stod(extractedData[i - 1][2]))* M_PI / 180; // Latitude 
		double lon2 = (stod(extractedData[i][1]))* M_PI / 180;   // Longitude 
		double lat2 = (stod(extractedData[i][2]))* M_PI / 180;   // Latitude 
		//计算方向变化角也要用弧度制
		double direction_angle = calculateDirectionAngle(lon1, lat1, lon2, lat2);
		cout << "lon1=" << lon1 << endl << "lat1=" << lat1 << endl << "lon2=" << lon2 << endl << "lat2=" << lat2 << endl;
		// Output Direction
		cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << direction_angle << " degrees" << endl;

		double lon2_1 = lon2 - lon1;
		double lat2_1 = lat2 - lat1;
		double lon_ = lon2_1 / 2;//1/2的Δlon
		double lat_ = lat2_1 / 2; //1 / 2的Δlat
		double sin2lon_ = sin(lon_)*sin(lon_);//sin²(1/2Δlon)
		double sin2lat_ = sin(lat_)*sin(lat_); //sin²(1 / 2Δlat)
		double cos_lat1 = cos(lat1);
		double cos_lat2 = cos(lat2);
		double sqrtA = sqrt(sin2lat_+ cos_lat1* cos_lat2*sin2lon_);
		//cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "sqrtA =" << sqrtA << endl;
		double asinA = asin(sqrtA);
		//长半轴 短半轴  单位是m
		int a_r = 6378137.0;
		int b_r = 6356752;
		double Earth_R = (2 * a_r + b_r) / 3;
		double Distance = 2 * Earth_R*asinA;
		cout << "Distance From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << Distance <<" meter"<< endl;
		int time = 3;//拍照间隔 s
		double speed = Distance / time;
		cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;
	}
	//cin.get();
	
	return 0;
}

四、结果展示

本文所有代码均由CSDN用户CV-X.WANG提供,任何个人或者团体,不得进行商用和教学活动,引用或部分引用,均需获得授权。

相关推荐
我是谁??2 分钟前
C/C++使用AddressSanitizer检测内存错误
c语言·c++
发霉的闲鱼35 分钟前
MFC 重写了listControl类(类名为A),并把双击事件的处理函数定义在A中,主窗口如何接收表格是否被双击
c++·mfc
小c君tt38 分钟前
MFC中Excel的导入以及使用步骤
c++·excel·mfc
xiaoxiao涛1 小时前
协程6 --- HOOK
c++·协程
大泽泽的小可爱2 小时前
NeurIPS24 | 多无人机协作精确预测车辆等目标移动轨迹, Drones Help Drones
无人机
羊小猪~~3 小时前
数据结构C语言描述2(图文结合)--有头单链表,无头单链表(两种方法),链表反转、有序链表构建、排序等操作,考研可看
c语言·数据结构·c++·考研·算法·链表·visual studio
脉牛杂德4 小时前
多项式加法——C语言
数据结构·c++·算法
legend_jz4 小时前
STL--哈希
c++·算法·哈希算法
CSUC4 小时前
【C++】父类参数有默认值时子类构造函数列表中可以省略该参数
c++
Vanranrr4 小时前
C++ QT
java·c++·qt