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

目录

一、引子

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

二、代码解释

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提供,任何个人或者团体,不得进行商用和教学活动,引用或部分引用,均需获得授权。

相关推荐
我们的五年14 分钟前
【Linux课程学习】:进程描述---PCB(Process Control Block)
linux·运维·c++
程序猿阿伟30 分钟前
《C++ 实现区块链:区块时间戳的存储与验证机制解析》
开发语言·c++·区块链
爱摸鱼的孔乙己1 小时前
【数据结构】链表(leetcode)
c语言·数据结构·c++·链表·csdn
烦躁的大鼻嘎2 小时前
模拟算法实例讲解:从理论到实践的编程之旅
数据结构·c++·算法·leetcode
IU宝2 小时前
C/C++内存管理
java·c语言·c++
fhvyxyci2 小时前
【C++之STL】摸清 string 的模拟实现(下)
开发语言·c++·string
C++忠实粉丝2 小时前
计算机网络socket编程(4)_TCP socket API 详解
网络·数据结构·c++·网络协议·tcp/ip·计算机网络·算法
古月居GYH2 小时前
在C++上实现反射用法
java·开发语言·c++
Betty’s Sweet2 小时前
[C++]:IO流
c++·文件·fstream·sstream·iostream
敲上瘾2 小时前
操作系统的理解
linux·运维·服务器·c++·大模型·操作系统·aigc