C++无人机摄影测量——OMEGA、Phi、kappa转换为YAW，Pitch，Roll

1 概述

任何处理三维旋转的人都需要熟悉欧拉角和旋转矩阵。欧拉角有助于以人类可以理解的方式描述三维旋转，因此在交互界面中很常见。另一方面，当涉及到在软件中实现高效旋转时，旋转矩阵是常用的表示。

不幸的是，在欧拉角和旋转矩阵之间来回转换是一个长期以来的混乱来源。原因并不是数学特别复杂，而是有几十种不同定义欧拉角的方法。不同的作者可能使用不同的约定，通常又不加以明确说明。这使得很难将来自多个源的公式和代码组合在一起。

欧拉角是Leonhard Euler引入的角度，用于描述刚体相对于固定坐标系的方向。也可以表示物理学中移动参考系的方向，或者表示三维线性代数中一般基的方向。经典欧拉角通常采用0度表示垂直方向的倾斜角度。后来Peter Guthrie Tait和George H.Bryan引入了Tait-Bryan形式，用于航空和工程，其中0度表示水平位置。

欧拉角是一组三角度，用于指定对象在三维空间中的方向。欧拉角三元组中的三个角中的每一个都指定了围绕三维笛卡尔坐标系中的一个轴的基本旋转（见图1）。

图 1：欧拉角轴、名称和符号约定旋转顺序为：(1) 偏航，(2) 俯仰，(3) 滚转。

2.Omega, Phi, Kappa 

OMEGA、phi、kappa角被定义为用于相对于大地坐标系（X、Y、Z）的旋转，并将其与图像坐标系对齐的角度。旋转按以下顺序应用：

• Kappa（κ），围绕Z轴旋转
• Phi（φ），绕Y轴旋转
• Omega（ω），绕Χ轴的旋转

图2 ω、φ、kappa角旋转（X、Y、Z）大地坐标系

3.代码实现：OMEGA、Phi、kappa转换为YAW，Pitch，Roll

（1）本代码需使用Eigen库，确保已正确安装Eigen

可以通过以下方式安装：https://eigen.tuxfamily.org/dox/GettingStarted.html

如果您有vcpkg，可按照如下操作完成。

在vcpkg根目录，打开powershell，输入“./vcpkg install eigen3”安装完成后，vcpkg 会将 Eigen 库的头文件和库文件安装到默认的位置。

（2）OMEGA、Phi、kappa转换为YAW，Pitch，Roll

#include <iostream>
#include <cmath>
#include <Eigen/Dense>



Eigen::Matrix3d eulerToRotationMatrix(double omega, double phi, double kappa) {
	Eigen::Matrix3d rotationMatrix;

	rotationMatrix(0, 0) = std::cos(phi) * std::cos(kappa);
	rotationMatrix(0, 1) = std::cos(omega) * std::sin(kappa) + std::sin(omega) * std::sin(phi) * std::cos(kappa);
	rotationMatrix(0, 2) = std::sin(omega) * std::sin(kappa) - std::cos(omega) * std::sin(phi) * std::cos(kappa);

	rotationMatrix(1, 0) = -std::cos(phi) * std::sin(kappa);
	rotationMatrix(1, 1) = std::cos(omega) * std::cos(kappa) - std::sin(omega) * std::sin(phi) * std::sin(kappa);
	rotationMatrix(1, 2) = std::sin(omega) * std::cos(kappa) + std::cos(omega) * std::sin(phi) * std::sin(kappa);

	rotationMatrix(2, 0) = std::sin(phi);
	rotationMatrix(2, 1) = -std::sin(omega) * std::cos(phi);
	rotationMatrix(2, 2) = std::cos(omega) * std::cos(phi);

	return rotationMatrix;
}

Eigen::Vector3d rotationMatrixToEulerAngles(const Eigen::Matrix3d& rotationMatrix) {
	Eigen::Vector3d euler;

	euler(1) = std::asin(-rotationMatrix(2, 0)); // Pitch

	if (std::cos(euler(1)) != 0) {
		euler(0) = std::atan2(rotationMatrix(2, 1), rotationMatrix(2, 2)); // Yaw
		euler(2) = std::atan2(rotationMatrix(1, 0), rotationMatrix(0, 0)); // Roll
	}
	else {
		euler(0) = 0;
		euler(2) = std::atan2(-rotationMatrix(0, 1), rotationMatrix(1, 1));
	}

	return euler;
}

int main() {
	double omega = 1.4922692413517904;  //在这里输入OMEGA、Phi、kappa的值
	double phi = -9.6371902267087872;
	double kappa = -80.2008829248658799;

	Eigen::Matrix3d rotationMatrix = eulerToRotationMatrix(omega, phi, kappa);
	Eigen::Vector3d euler = rotationMatrixToEulerAngles(rotationMatrix);

	std::cout << "Yaw: " << euler(0) << std::endl;
	std::cout << "Pitch: " << euler(1) << std::endl;
	std::cout << "Roll: " << euler(2) << std::endl;

	return 0;
}

本文内容参考：欧拉角简介