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

目录

一、引子

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

二、代码解释

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

2.定义圆周率数值

3.定义方向计算函数

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

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

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

三、完整代码展示

四、结果展示

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

一、引子

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

二、代码解释

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

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

2.定义圆周率数值

#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;

为弧度制。

//方向函数
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  
4.从txt文件提取所需信息
 
	ifstream file("LBH.txt");
	if (!file.is_open()) {
		cerr